Merge pull request #12678 from AndreaCatania/soft

Soft body

31 files changed, 2085 insertions, 225 deletions

diff --git a/core/vector.h b/core/vector.h
index f586471e27..c026448ddd 100644
--- a/core/vector.h
+++ b/core/vector.h
@@ -152,6 +152,8 @@ public:
 
 	Error insert(int p_pos, const T &p_val);
 
+	void append_array(const Vector<T> &p_other);
+
 	template <class C>
 	void sort_custom() {
 
@@ -408,6 +410,17 @@ Error Vector<T>::insert(int p_pos, const T &p_val) {
 }
 
 template <class T>
+void Vector<T>::append_array(const Vector<T> &p_other) {
+	const int ds = p_other.size();
+	if (ds == 0)
+		return;
+	const int bs = size();
+	resize(bs + ds);
+	for (int i = 0; i < ds; ++i)
+		operator[](bs + i) = p_other[i];
+}
+
+template <class T>
 Vector<T>::Vector(const Vector &p_from) {
 
 	_ptr = NULL;
diff --git a/drivers/gles3/rasterizer_storage_gles3.cpp b/drivers/gles3/rasterizer_storage_gles3.cpp
index e67b0bea21..9e389a353e 100644
--- a/drivers/gles3/rasterizer_storage_gles3.cpp
+++ b/drivers/gles3/rasterizer_storage_gles3.cpp
@@ -3240,7 +3240,7 @@ void RasterizerStorageGLES3::mesh_surface_update_region(RID p_mesh, int p_surfac
 
 	PoolVector<uint8_t>::Read r = p_data.read();
 
-	glBindBuffer(GL_ARRAY_BUFFER, mesh->surfaces[p_surface]->array_id);
+	glBindBuffer(GL_ARRAY_BUFFER, mesh->surfaces[p_surface]->vertex_id);
 	glBufferSubData(GL_ARRAY_BUFFER, p_offset, total_size, r.ptr());
 	glBindBuffer(GL_ARRAY_BUFFER, 0); //unbind
 }
@@ -3404,6 +3404,7 @@ Vector<PoolVector<uint8_t> > RasterizerStorageGLES3::mesh_surface_get_blend_shap
 
 	return bsarr;
 }
+
 Vector<AABB> RasterizerStorageGLES3::mesh_surface_get_skeleton_aabb(RID p_mesh, int p_surface) const {
 
 	const Mesh *mesh = mesh_owner.getornull(p_mesh);
@@ -3455,6 +3456,7 @@ void RasterizerStorageGLES3::mesh_remove_surface(RID p_mesh, int p_surface) {
 
 	mesh->instance_change_notify();
 }
+
 int RasterizerStorageGLES3::mesh_get_surface_count(RID p_mesh) const {
 
 	const Mesh *mesh = mesh_owner.getornull(p_mesh);
@@ -3468,6 +3470,7 @@ void RasterizerStorageGLES3::mesh_set_custom_aabb(RID p_mesh, const AABB &p_aabb
 	ERR_FAIL_COND(!mesh);
 
 	mesh->custom_aabb = p_aabb;
+	mesh->instance_change_notify();
 }
 
 AABB RasterizerStorageGLES3::mesh_get_custom_aabb(RID p_mesh) const {
diff --git a/drivers/gles3/rasterizer_storage_gles3.h b/drivers/gles3/rasterizer_storage_gles3.h
index 80df21941b..1db577f23c 100644
--- a/drivers/gles3/rasterizer_storage_gles3.h
+++ b/drivers/gles3/rasterizer_storage_gles3.h
@@ -693,7 +693,6 @@ public:
 		AABB custom_aabb;
 		mutable uint64_t last_pass;
 		SelfList<MultiMesh>::List multimeshes;
-
 		_FORCE_INLINE_ void update_multimeshes() {
 
 			SelfList<MultiMesh> *mm = multimeshes.first();
diff --git a/editor/icons/icon_soft_body.svg b/editor/icons/icon_soft_body.svg
new file mode 100644
index 0000000000..9930026b61
--- /dev/null
+++ b/editor/icons/icon_soft_body.svg
@@ -0,0 +1,56 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<svg
+   xmlns:dc="http://purl.org/dc/elements/1.1/"
+   xmlns:cc="http://creativecommons.org/ns#"
+   xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
+   xmlns:svg="http://www.w3.org/2000/svg"
+   xmlns="http://www.w3.org/2000/svg"
+   xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
+   xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
+   width="16"
+   height="16"
+   version="1.1"
+   viewBox="0 0 16 16"
+   id="svg2"
+   inkscape:version="0.91 r13725"
+   sodipodi:docname="icon_soft_body.svg">
+  <metadata
+     id="metadata14">
+    <rdf:RDF>
+      <cc:Work
+         rdf:about="">
+        <dc:format>image/svg+xml</dc:format>
+        <dc:type
+           rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
+      </cc:Work>
+    </rdf:RDF>
+  </metadata>
+  <defs
+     id="defs12" />
+  <sodipodi:namedview
+     pagecolor="#ffffff"
+     bordercolor="#666666"
+     borderopacity="1"
+     objecttolerance="10"
+     gridtolerance="10"
+     guidetolerance="10"
+     inkscape:pageopacity="0"
+     inkscape:pageshadow="2"
+     inkscape:window-width="1920"
+     inkscape:window-height="1027"
+     id="namedview10"
+     showgrid="false"
+     inkscape:zoom="18.792233"
+     inkscape:cx="2.8961304"
+     inkscape:cy="4.3816933"
+     inkscape:window-x="-8"
+     inkscape:window-y="-8"
+     inkscape:window-maximized="1"
+     inkscape:current-layer="svg2" />
+  <path
+     style="opacity:1;fill:#fc9c9c;fill-opacity:0.99607843;fill-rule:nonzero;stroke:none;stroke-width:1.42799997;stroke-linecap:square;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-dashoffset:0;stroke-opacity:1"
+     d="m 2.3447105,1.6091897 c -0.011911,1.9816766 -1.4168958,3.9344766 0,5.9495986 1.4168957,2.0151221 0,6.6693597 0,6.6693597 l 10.9510055,0 c 0,0 1.780829,-4.4523824 0,-6.489075 -1.780829,-2.0366925 -0.183458,-4.119112 0,-6.1298833 z m 1.8894067,0.7549031 7.4390658,0 c -0.431995,1.5996085 -1.62289,4.0426807 0,5.3749802 1.622888,1.3322996 0,5.887932 0,5.887932 l -7.4390658,0 c 0,0 1.3903413,-4.3680495 0,-5.9780743 -1.3903412,-1.6100247 -0.3951213,-3.7149271 0,-5.2848379 z"
+     id="rect4142"
+     inkscape:connector-curvature="0"
+     sodipodi:nodetypes="czcczcccczcczc" />
+</svg>
diff --git a/editor/plugins/spatial_editor_plugin.h b/editor/plugins/spatial_editor_plugin.h
index 637926a913..af882f6e05 100644
--- a/editor/plugins/spatial_editor_plugin.h
+++ b/editor/plugins/spatial_editor_plugin.h
@@ -60,6 +60,7 @@ public:
 	virtual Variant get_handle_value(int p_idx) const;
 	virtual void set_handle(int p_idx, Camera *p_camera, const Point2 &p_point);
 	virtual void commit_handle(int p_idx, const Variant &p_restore, bool p_cancel = false);
+	virtual bool is_gizmo_handle_highlighted(int idx) const { return false; }
 
 	virtual bool intersect_frustum(const Camera *p_camera, const Vector<Plane> &p_frustum);
 	virtual bool intersect_ray(Camera *p_camera, const Point2 &p_point, Vector3 &r_pos, Vector3 &r_normal, int *r_gizmo_handle = NULL, bool p_sec_first = false);
diff --git a/editor/spatial_editor_gizmos.cpp b/editor/spatial_editor_gizmos.cpp
index c45dea0df7..35544f711b 100644
--- a/editor/spatial_editor_gizmos.cpp
+++ b/editor/spatial_editor_gizmos.cpp
@@ -33,6 +33,7 @@
 #include "geometry.h"
 #include "quick_hull.h"
 #include "scene/3d/camera.h"
+#include "scene/3d/soft_body.h"
 #include "scene/resources/box_shape.h"
 #include "scene/resources/capsule_shape.h"
 #include "scene/resources/convex_polygon_shape.h"
@@ -256,8 +257,12 @@ void EditorSpatialGizmo::add_handles(const Vector<Vector3> &p_handles, bool p_bi
 		for (int i = 0; i < p_handles.size(); i++) {
 
 			Color col(1, 1, 1, 1);
+			if (is_gizmo_handle_highlighted(i))
+				col = Color(0, 0, 1, 0.9);
+
 			if (SpatialEditor::get_singleton()->get_over_gizmo_handle() != i)
-				col = Color(0.9, 0.9, 0.9, 0.9);
+				col.a = 0.8;
+
 			w[i] = col;
 		}
 	}
@@ -1914,6 +1919,100 @@ VehicleWheelSpatialGizmo::VehicleWheelSpatialGizmo(VehicleWheel *p_car_wheel) {
 
 ///////////
 
+void SoftBodySpatialGizmo::redraw() {
+	clear();
+
+	if (!soft_body || soft_body->get_mesh().is_null()) {
+		return;
+	}
+
+	// find mesh
+
+	Vector<Vector3> lines;
+
+	soft_body->get_mesh()->generate_debug_mesh_lines(lines);
+
+	if (!lines.size()) {
+		return;
+	}
+
+	Vector<Vector3> points;
+	soft_body->get_mesh()->generate_debug_mesh_indices(points);
+
+	soft_body->get_mesh()->clear_cache();
+
+	Color gizmo_color = EDITOR_GET("editors/3d_gizmos/gizmo_colors/shape");
+	Ref<Material> material = create_material("shape_material", gizmo_color);
+
+	add_lines(lines, material);
+	add_collision_segments(lines);
+	add_handles(points);
+}
+
+bool SoftBodySpatialGizmo::intersect_ray(Camera *p_camera, const Point2 &p_point, Vector3 &r_pos, Vector3 &r_normal, int *r_gizmo_handle, bool p_sec_first) {
+	return EditorSpatialGizmo::intersect_ray(p_camera, p_point, r_pos, r_normal, r_gizmo_handle, p_sec_first);
+
+	/* Perform a shape cast but doesn't work with softbody
+	PhysicsDirectSpaceState *space_state = PhysicsServer::get_singleton()->space_get_direct_state(SceneTree::get_singleton()->get_root()->get_world()->get_space());
+	if (!physics_sphere_shape.is_valid()) {
+		physics_sphere_shape = PhysicsServer::get_singleton()->shape_create(PhysicsServer::SHAPE_SPHERE);
+		real_t radius = 0.02;
+		PhysicsServer::get_singleton()->shape_set_data(physics_sphere_shape, radius);
+	}
+
+	Vector3 sphere_motion(p_camera->project_ray_normal(p_point));
+	real_t closest_safe;
+	real_t closest_unsafe;
+	PhysicsDirectSpaceState::ShapeRestInfo result;
+	bool collided = space_state->cast_motion(
+			physics_sphere_shape,
+			p_camera->get_transform(),
+			sphere_motion * Vector3(1000, 1000, 1000),
+			0.f,
+			closest_safe,
+			closest_unsafe,
+			Set<RID>(),
+			0xFFFFFFFF,
+			0xFFFFFFFF,
+			&result);
+
+	if (collided) {
+
+		if (result.collider_id == soft_body->get_instance_id()) {
+			print_line("Collided");
+		} else {
+			print_line("Collided but with wrong object: " + itos(result.collider_id));
+		}
+	} else {
+		print_line("Not collided, motion: x: " + rtos(sphere_motion[0]) + " y: " + rtos(sphere_motion[1]) + " z: " + rtos(sphere_motion[2]));
+	}
+	return false;
+	*/
+}
+
+void SoftBodySpatialGizmo::commit_handle(int p_idx, const Variant &p_restore, bool p_cancel) {
+	soft_body->pin_point_toggle(p_idx);
+	redraw();
+}
+
+bool SoftBodySpatialGizmo::is_gizmo_handle_highlighted(int idx) const {
+	return soft_body->is_point_pinned(idx);
+}
+
+SoftBodySpatialGizmo::SoftBodySpatialGizmo(SoftBody *p_soft_physics_body) :
+		EditorSpatialGizmo(),
+		soft_body(p_soft_physics_body) {
+	set_spatial_node(p_soft_physics_body);
+}
+
+SoftBodySpatialGizmo::~SoftBodySpatialGizmo() {
+	//if (!physics_sphere_shape.is_valid()) {
+	//	PhysicsServer::get_singleton()->free(physics_sphere_shape);
+	//}
+}
+
+///////////
+
 String CollisionShapeSpatialGizmo::get_handle_name(int p_idx) const {
 
 	Ref<Shape> s = cs->get_shape();
@@ -4051,6 +4150,12 @@ Ref<SpatialEditorGizmo> SpatialEditorGizmos::get_gizmo(Spatial *p_spatial) {
 		return lsg;
 	}
 
+	if (Object::cast_to<SoftBody>(p_spatial)) {
+
+		Ref<SoftBodySpatialGizmo> misg = memnew(SoftBodySpatialGizmo(Object::cast_to<SoftBody>(p_spatial)));
+		return misg;
+	}
+
 	if (Object::cast_to<MeshInstance>(p_spatial)) {
 
 		Ref<MeshInstanceSpatialGizmo> misg = memnew(MeshInstanceSpatialGizmo(Object::cast_to<MeshInstance>(p_spatial)));
@@ -4081,6 +4186,7 @@ Ref<SpatialEditorGizmo> SpatialEditorGizmos::get_gizmo(Spatial *p_spatial) {
 		return misg;
 	}
 */
+
 	if (Object::cast_to<CollisionShape>(p_spatial)) {
 
 		Ref<CollisionShapeSpatialGizmo> misg = memnew(CollisionShapeSpatialGizmo(Object::cast_to<CollisionShape>(p_spatial)));
diff --git a/editor/spatial_editor_gizmos.h b/editor/spatial_editor_gizmos.h
index 924f82dc16..198d028516 100644
--- a/editor/spatial_editor_gizmos.h
+++ b/editor/spatial_editor_gizmos.h
@@ -331,6 +331,23 @@ public:
 	BakedIndirectLightGizmo(BakedLightmap *p_baker = NULL);
 };
 
+class SoftBodySpatialGizmo : public EditorSpatialGizmo {
+	GDCLASS(SoftBodySpatialGizmo, EditorSpatialGizmo);
+
+	class SoftBody *soft_body;
+	//RID physics_sphere_shape; // Used for raycast that doesn't work, in this moment, with softbody
+
+public:
+	void redraw();
+	virtual bool intersect_ray(Camera *p_camera, const Point2 &p_point, Vector3 &r_pos, Vector3 &r_normal, int *r_gizmo_handle = NULL, bool p_sec_first = false);
+	virtual void commit_handle(int p_idx, const Variant &p_restore, bool p_cancel);
+
+	virtual bool is_gizmo_handle_highlighted(int idx) const;
+
+	SoftBodySpatialGizmo(SoftBody *p_soft_physics_body = NULL);
+	~SoftBodySpatialGizmo();
+};
+
 class CollisionShapeSpatialGizmo : public EditorSpatialGizmo {
 
 	GDCLASS(CollisionShapeSpatialGizmo, EditorSpatialGizmo);
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; }
diff --git a/scene/3d/collision_object.h b/scene/3d/collision_object.h
index f31d65e411..f8ef04b78f 100644
--- a/scene/3d/collision_object.h
+++ b/scene/3d/collision_object.h
@@ -39,6 +39,7 @@ class CollisionObject : public Spatial {
 	GDCLASS(CollisionObject, Spatial);
 
 	bool area;
+
 	RID rid;
 
 	struct ShapeData {
diff --git a/scene/3d/collision_shape.h b/scene/3d/collision_shape.h
index c9c91a5824..6ca8e80ea1 100644
--- a/scene/3d/collision_shape.h
+++ b/scene/3d/collision_shape.h
@@ -49,6 +49,7 @@ class CollisionShape : public Spatial {
 	void resource_changed(RES res);
 	bool disabled;
 
+protected:
 	void _create_debug_shape();
 
 	void _update_in_shape_owner(bool p_xform_only = false);
diff --git a/scene/3d/mesh_instance.cpp b/scene/3d/mesh_instance.cpp
index e836a6154a..722eadb9ca 100644
--- a/scene/3d/mesh_instance.cpp
+++ b/scene/3d/mesh_instance.cpp
@@ -36,6 +36,7 @@
 #include "scene/resources/material.h"
 #include "scene/scene_string_names.h"
 #include "skeleton.h"
+
 bool MeshInstance::_set(const StringName &p_name, const Variant &p_value) {
 
 	//this is not _too_ bad performance wise, really. it only arrives here if the property was not set anywhere else.
diff --git a/scene/3d/mesh_instance.h b/scene/3d/mesh_instance.h
index 5d359cd4d5..0dfec538f9 100644
--- a/scene/3d/mesh_instance.h
+++ b/scene/3d/mesh_instance.h
@@ -41,6 +41,7 @@ class MeshInstance : public GeometryInstance {
 
 	GDCLASS(MeshInstance, GeometryInstance);
 
+protected:
 	Ref<Mesh> mesh;
 	NodePath skeleton_path;
 
diff --git a/scene/3d/physics_body.cpp b/scene/3d/physics_body.cpp
index 8c5d4da117..7529a0e524 100644
--- a/scene/3d/physics_body.cpp
+++ b/scene/3d/physics_body.cpp
@@ -122,23 +122,23 @@ bool PhysicsBody::get_collision_layer_bit(int p_bit) const {
 void PhysicsBody::add_collision_exception_with(Node *p_node) {
 
 	ERR_FAIL_NULL(p_node);
-	PhysicsBody *physics_body = Object::cast_to<PhysicsBody>(p_node);
-	if (!physics_body) {
-		ERR_EXPLAIN("Collision exception only works between two objects of PhysicsBody type");
+	CollisionObject *collision_object = Object::cast_to<CollisionObject>(p_node);
+	if (!collision_object) {
+		ERR_EXPLAIN("Collision exception only works between two CollisionObject");
 	}
-	ERR_FAIL_COND(!physics_body);
-	PhysicsServer::get_singleton()->body_add_collision_exception(get_rid(), physics_body->get_rid());
+	ERR_FAIL_COND(!collision_object);
+	PhysicsServer::get_singleton()->body_add_collision_exception(get_rid(), collision_object->get_rid());
 }
 
 void PhysicsBody::remove_collision_exception_with(Node *p_node) {
 
 	ERR_FAIL_NULL(p_node);
-	PhysicsBody *physics_body = Object::cast_to<PhysicsBody>(p_node);
-	if (!physics_body) {
-		ERR_EXPLAIN("Collision exception only works between two objects of PhysicsBody type");
+	CollisionObject *collision_object = Object::cast_to<CollisionObject>(p_node);
+	if (!collision_object) {
+		ERR_EXPLAIN("Collision exception only works between two CollisionObject");
 	}
-	ERR_FAIL_COND(!physics_body);
-	PhysicsServer::get_singleton()->body_remove_collision_exception(get_rid(), physics_body->get_rid());
+	ERR_FAIL_COND(!collision_object);
+	PhysicsServer::get_singleton()->body_remove_collision_exception(get_rid(), collision_object->get_rid());
 }
 
 void PhysicsBody::_set_layers(uint32_t p_mask) {
diff --git a/scene/3d/soft_body.cpp b/scene/3d/soft_body.cpp
new file mode 100644
index 0000000000..8498dc34c0
--- /dev/null
+++ b/scene/3d/soft_body.cpp
@@ -0,0 +1,740 @@
+/*************************************************************************/
+/*  soft_physics_body.cpp                                                */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 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.h"
+#include "os/os.h"
+#include "scene/3d/collision_object.h"
+#include "scene/3d/skeleton.h"
+#include "servers/physics_server.h"
+
+SoftBodyVisualServerHandler::SoftBodyVisualServerHandler() {}
+
+void SoftBodyVisualServerHandler::prepare(RID p_mesh, int p_surface) {
+	clear();
+
+	ERR_FAIL_COND(!p_mesh.is_valid());
+
+	mesh = p_mesh;
+	surface = p_surface;
+
+	const uint32_t surface_format = VS::get_singleton()->mesh_surface_get_format(mesh, surface);
+	const int surface_vertex_len = VS::get_singleton()->mesh_surface_get_array_len(mesh, p_surface);
+	const int surface_index_len = VS::get_singleton()->mesh_surface_get_array_index_len(mesh, p_surface);
+	uint32_t surface_offsets[VS::ARRAY_MAX];
+
+	buffer = VS::get_singleton()->mesh_surface_get_array(mesh, surface);
+	stride = VS::get_singleton()->mesh_surface_make_offsets_from_format(surface_format, surface_vertex_len, surface_index_len, surface_offsets);
+	offset_vertices = surface_offsets[VS::ARRAY_VERTEX];
+	offset_normal = surface_offsets[VS::ARRAY_NORMAL];
+}
+
+void SoftBodyVisualServerHandler::clear() {
+
+	if (mesh.is_valid()) {
+		buffer.resize(0);
+	}
+
+	mesh = RID();
+}
+
+void SoftBodyVisualServerHandler::open() {
+	write_buffer = buffer.write();
+}
+
+void SoftBodyVisualServerHandler::close() {
+	write_buffer = PoolVector<uint8_t>::Write();
+}
+
+void SoftBodyVisualServerHandler::commit_changes() {
+	VS::get_singleton()->mesh_surface_update_region(mesh, surface, 0, buffer);
+}
+
+void SoftBodyVisualServerHandler::set_vertex(int p_vertex_id, const void *p_vector3) {
+	copymem(&write_buffer[p_vertex_id * stride + offset_vertices], p_vector3, sizeof(float) * 3);
+}
+
+void SoftBodyVisualServerHandler::set_normal(int p_vertex_id, const void *p_vector3) {
+	copymem(&write_buffer[p_vertex_id * stride + offset_normal], p_vector3, sizeof(float) * 3);
+}
+
+void SoftBodyVisualServerHandler::set_aabb(const AABB &p_aabb) {
+	VS::get_singleton()->mesh_set_custom_aabb(mesh, p_aabb);
+}
+
+SoftBody::PinnedPoint::PinnedPoint() :
+		point_index(-1),
+		spatial_attachment(NULL) {
+}
+
+SoftBody::PinnedPoint::PinnedPoint(const PinnedPoint &obj_tocopy) {
+	point_index = obj_tocopy.point_index;
+	spatial_attachment_path = obj_tocopy.spatial_attachment_path;
+	spatial_attachment = obj_tocopy.spatial_attachment;
+	vertex_offset_transform = obj_tocopy.vertex_offset_transform;
+}
+
+void SoftBody::_update_pickable() {
+	if (!is_inside_tree())
+		return;
+	bool pickable = ray_pickable && is_inside_tree() && is_visible_in_tree();
+	PhysicsServer::get_singleton()->soft_body_set_ray_pickable(physics_rid, pickable);
+}
+
+bool SoftBody::_set(const StringName &p_name, const Variant &p_value) {
+	String name = p_name;
+	String which = name.get_slicec('/', 0);
+
+	if ("pinned_points" == which) {
+
+		return _set_property_pinned_points_indices(p_value);
+
+	} else if ("attachments" == which) {
+
+		int idx = name.get_slicec('/', 1).to_int();
+		String what = name.get_slicec('/', 2);
+
+		return _set_property_pinned_points_attachment(idx, what, p_value);
+	}
+
+	return false;
+}
+
+bool SoftBody::_get(const StringName &p_name, Variant &r_ret) const {
+	String name = p_name;
+	String which = name.get_slicec('/', 0);
+
+	if ("pinned_points" == which) {
+		Array arr_ret;
+		const int pinned_points_indices_size = pinned_points_indices.size();
+		PoolVector<PinnedPoint>::Read r = pinned_points_indices.read();
+		arr_ret.resize(pinned_points_indices_size);
+
+		for (int i = 0; i < pinned_points_indices_size; ++i) {
+			arr_ret[i] = r[i].point_index;
+		}
+
+		r_ret = arr_ret;
+		return true;
+
+	} else if ("attachments" == which) {
+
+		int idx = name.get_slicec('/', 1).to_int();
+		String what = name.get_slicec('/', 2);
+
+		return _get_property_pinned_points(idx, what, r_ret);
+	}
+
+	return false;
+}
+
+void SoftBody::_get_property_list(List<PropertyInfo> *p_list) const {
+
+	const int pinned_points_indices_size = pinned_points_indices.size();
+
+	p_list->push_back(PropertyInfo(Variant::POOL_INT_ARRAY, "pinned_points"));
+
+	for (int i = 0; i < pinned_points_indices_size; ++i) {
+		p_list->push_back(PropertyInfo(Variant::INT, "attachments/" + itos(i) + "/point_index"));
+		p_list->push_back(PropertyInfo(Variant::NODE_PATH, "attachments/" + itos(i) + "/spatial_attachment_path"));
+	}
+}
+
+bool SoftBody::_set_property_pinned_points_indices(const Array &p_indices) {
+
+	const int p_indices_size = p_indices.size();
+
+	{ // Remove the pined points on physics server that will be removed by resize
+		PoolVector<PinnedPoint>::Read r = pinned_points_indices.read();
+		if (p_indices_size < pinned_points_indices.size()) {
+			for (int i = pinned_points_indices.size() - 1; i >= p_indices_size; --i) {
+				pin_point(r[i].point_index, false);
+			}
+		}
+	}
+
+	pinned_points_indices.resize(p_indices_size);
+
+	PoolVector<PinnedPoint>::Write w = pinned_points_indices.write();
+	int point_index;
+	for (int i = 0; i < p_indices_size; ++i) {
+		point_index = p_indices.get(i);
+		if (w[i].point_index != point_index) {
+			if (-1 != w[i].point_index)
+				pin_point(w[i].point_index, false);
+			w[i].point_index = point_index;
+			pin_point(w[i].point_index, true);
+		}
+	}
+	return true;
+}
+
+bool SoftBody::_set_property_pinned_points_attachment(int p_item, const String &p_what, const Variant &p_value) {
+	if (pinned_points_indices.size() <= p_item) {
+		return false;
+	}
+
+	if ("spatial_attachment_path" == p_what) {
+		PoolVector<PinnedPoint>::Write w = pinned_points_indices.write();
+		pin_point(w[p_item].point_index, true, p_value);
+	} else {
+		return false;
+	}
+
+	return true;
+}
+
+bool SoftBody::_get_property_pinned_points(int p_item, const String &p_what, Variant &r_ret) const {
+	if (pinned_points_indices.size() <= p_item) {
+		return false;
+	}
+	PoolVector<PinnedPoint>::Read r = pinned_points_indices.read();
+
+	if ("point_index" == p_what) {
+		r_ret = r[p_item].point_index;
+	} else if ("spatial_attachment_path" == p_what) {
+		r_ret = r[p_item].spatial_attachment_path;
+	} else {
+		return false;
+	}
+
+	return true;
+}
+
+void SoftBody::_changed_callback(Object *p_changed, const char *p_prop) {
+#ifdef TOOLS_ENABLED
+	if (p_changed == this) {
+		update_configuration_warning();
+	}
+#endif
+}
+
+void SoftBody::_notification(int p_what) {
+	switch (p_what) {
+		case NOTIFICATION_ENTER_WORLD: {
+
+			if (Engine::get_singleton()->is_editor_hint())
+				add_change_receptor(this);
+
+			RID space = get_world()->get_space();
+			PhysicsServer::get_singleton()->soft_body_set_space(physics_rid, space);
+			PhysicsServer::get_singleton()->soft_body_set_transform(physics_rid, get_global_transform());
+			update_physics_server();
+		} break;
+		case NOTIFICATION_READY: {
+			if (!parent_collision_ignore.is_empty())
+				add_collision_exception_with(get_node(parent_collision_ignore));
+
+		} break;
+		case NOTIFICATION_TRANSFORM_CHANGED: {
+
+			if (!simulation_started) {
+				PhysicsServer::get_singleton()->soft_body_set_transform(physics_rid, get_global_transform());
+
+				_update_cache_pin_points_datas();
+				// Submit bone attachment
+				const int pinned_points_indices_size = pinned_points_indices.size();
+				PoolVector<PinnedPoint>::Read r = pinned_points_indices.read();
+				for (int i = 0; i < pinned_points_indices_size; ++i) {
+					if (!r[i].spatial_attachment) {
+						// Use soft body position to update the point position
+						PhysicsServer::get_singleton()->soft_body_move_point(physics_rid, r[i].point_index, (get_global_transform() * r[i].vertex_offset_transform).origin);
+					} else {
+						PhysicsServer::get_singleton()->soft_body_move_point(physics_rid, r[i].point_index, (r[i].spatial_attachment->get_global_transform() * r[i].vertex_offset_transform).origin);
+					}
+				}
+			}
+		} break;
+		case NOTIFICATION_VISIBILITY_CHANGED: {
+
+			_update_pickable();
+
+		} break;
+		case NOTIFICATION_EXIT_WORLD: {
+
+			PhysicsServer::get_singleton()->soft_body_set_space(physics_rid, RID());
+
+		} break;
+	}
+
+#ifdef TOOLS_ENABLED
+
+	if (p_what == NOTIFICATION_LOCAL_TRANSFORM_CHANGED) {
+		if (Engine::get_singleton()->is_editor_hint()) {
+			update_configuration_warning();
+		}
+	}
+
+#endif
+}
+
+void SoftBody::_bind_methods() {
+
+	ClassDB::bind_method(D_METHOD("_draw_soft_mesh"), &SoftBody::_draw_soft_mesh);
+
+	ClassDB::bind_method(D_METHOD("set_collision_mask", "collision_mask"), &SoftBody::set_collision_mask);
+	ClassDB::bind_method(D_METHOD("get_collision_mask"), &SoftBody::get_collision_mask);
+
+	ClassDB::bind_method(D_METHOD("set_collision_layer", "collision_layer"), &SoftBody::set_collision_layer);
+	ClassDB::bind_method(D_METHOD("get_collision_layer"), &SoftBody::get_collision_layer);
+
+	ClassDB::bind_method(D_METHOD("set_collision_mask_bit", "bit", "value"), &SoftBody::set_collision_mask_bit);
+	ClassDB::bind_method(D_METHOD("get_collision_mask_bit", "bit"), &SoftBody::get_collision_mask_bit);
+
+	ClassDB::bind_method(D_METHOD("set_collision_layer_bit", "bit", "value"), &SoftBody::set_collision_layer_bit);
+	ClassDB::bind_method(D_METHOD("get_collision_layer_bit", "bit"), &SoftBody::get_collision_layer_bit);
+
+	ClassDB::bind_method(D_METHOD("set_parent_collision_ignore", "parent_collision_ignore"), &SoftBody::set_parent_collision_ignore);
+	ClassDB::bind_method(D_METHOD("get_parent_collision_ignore"), &SoftBody::get_parent_collision_ignore);
+
+	ClassDB::bind_method(D_METHOD("add_collision_exception_with", "body"), &SoftBody::add_collision_exception_with);
+	ClassDB::bind_method(D_METHOD("remove_collision_exception_with", "body"), &SoftBody::remove_collision_exception_with);
+
+	ClassDB::bind_method(D_METHOD("set_simulation_precision", "simulation_precision"), &SoftBody::set_simulation_precision);
+	ClassDB::bind_method(D_METHOD("get_simulation_precision"), &SoftBody::get_simulation_precision);
+
+	ClassDB::bind_method(D_METHOD("set_total_mass", "mass"), &SoftBody::set_total_mass);
+	ClassDB::bind_method(D_METHOD("get_total_mass"), &SoftBody::get_total_mass);
+
+	ClassDB::bind_method(D_METHOD("set_linear_stiffness", "linear_stiffness"), &SoftBody::set_linear_stiffness);
+	ClassDB::bind_method(D_METHOD("get_linear_stiffness"), &SoftBody::get_linear_stiffness);
+
+	ClassDB::bind_method(D_METHOD("set_areaAngular_stiffness", "areaAngular_stiffness"), &SoftBody::set_areaAngular_stiffness);
+	ClassDB::bind_method(D_METHOD("get_areaAngular_stiffness"), &SoftBody::get_areaAngular_stiffness);
+
+	ClassDB::bind_method(D_METHOD("set_volume_stiffness", "volume_stiffness"), &SoftBody::set_volume_stiffness);
+	ClassDB::bind_method(D_METHOD("get_volume_stiffness"), &SoftBody::get_volume_stiffness);
+
+	ClassDB::bind_method(D_METHOD("set_pressure_coefficient", "pressure_coefficient"), &SoftBody::set_pressure_coefficient);
+	ClassDB::bind_method(D_METHOD("get_pressure_coefficient"), &SoftBody::get_pressure_coefficient);
+
+	ClassDB::bind_method(D_METHOD("set_pose_matching_coefficient", "pose_matching_coefficient"), &SoftBody::set_pose_matching_coefficient);
+	ClassDB::bind_method(D_METHOD("get_pose_matching_coefficient"), &SoftBody::get_pose_matching_coefficient);
+
+	ClassDB::bind_method(D_METHOD("set_damping_coefficient", "damping_coefficient"), &SoftBody::set_damping_coefficient);
+	ClassDB::bind_method(D_METHOD("get_damping_coefficient"), &SoftBody::get_damping_coefficient);
+
+	ClassDB::bind_method(D_METHOD("set_drag_coefficient", "drag_coefficient"), &SoftBody::set_drag_coefficient);
+	ClassDB::bind_method(D_METHOD("get_drag_coefficient"), &SoftBody::get_drag_coefficient);
+
+	ClassDB::bind_method(D_METHOD("set_ray_pickable", "ray_pickable"), &SoftBody::set_ray_pickable);
+	ClassDB::bind_method(D_METHOD("is_ray_pickable"), &SoftBody::is_ray_pickable);
+
+	ADD_GROUP("Collision", "collision_");
+	ADD_PROPERTY(PropertyInfo(Variant::INT, "collision_layer", PROPERTY_HINT_LAYERS_3D_PHYSICS), "set_collision_layer", "get_collision_layer");
+	ADD_PROPERTY(PropertyInfo(Variant::INT, "collision_mask", PROPERTY_HINT_LAYERS_3D_PHYSICS), "set_collision_mask", "get_collision_mask");
+
+	ADD_PROPERTY(PropertyInfo(Variant::NODE_PATH, "parent_collision_ignore", PROPERTY_HINT_PROPERTY_OF_VARIANT_TYPE, "Parent collision object"), "set_parent_collision_ignore", "get_parent_collision_ignore");
+	ADD_PROPERTY(PropertyInfo(Variant::INT, "simulation_precision", PROPERTY_HINT_RANGE, "1,100,1"), "set_simulation_precision", "get_simulation_precision");
+	ADD_PROPERTY(PropertyInfo(Variant::REAL, "total_mass", PROPERTY_HINT_RANGE, "0.01,10000,1"), "set_total_mass", "get_total_mass");
+	ADD_PROPERTY(PropertyInfo(Variant::REAL, "linear_stiffness", PROPERTY_HINT_RANGE, "0,1,0.01"), "set_linear_stiffness", "get_linear_stiffness");
+	ADD_PROPERTY(PropertyInfo(Variant::REAL, "areaAngular_stiffness", PROPERTY_HINT_RANGE, "0,1,0.01"), "set_areaAngular_stiffness", "get_areaAngular_stiffness");
+	ADD_PROPERTY(PropertyInfo(Variant::REAL, "volume_stiffness", PROPERTY_HINT_RANGE, "0,1,0.01"), "set_volume_stiffness", "get_volume_stiffness");
+	ADD_PROPERTY(PropertyInfo(Variant::REAL, "pressure_coefficient"), "set_pressure_coefficient", "get_pressure_coefficient");
+	ADD_PROPERTY(PropertyInfo(Variant::REAL, "damping_coefficient", PROPERTY_HINT_RANGE, "0,1,0.01"), "set_damping_coefficient", "get_damping_coefficient");
+	ADD_PROPERTY(PropertyInfo(Variant::REAL, "drag_coefficient", PROPERTY_HINT_RANGE, "0,1,0.01"), "set_drag_coefficient", "get_drag_coefficient");
+	ADD_PROPERTY(PropertyInfo(Variant::REAL, "pose_matching_coefficient", PROPERTY_HINT_RANGE, "0,1,0.01"), "set_pose_matching_coefficient", "get_pose_matching_coefficient");
+}
+
+String SoftBody::get_configuration_warning() const {
+
+	String warning = MeshInstance::get_configuration_warning();
+
+	if (get_mesh().is_null()) {
+		if (!warning.empty())
+			warning += "\n\n";
+
+		warning += TTR("This body will be ignored until you set a mesh");
+	}
+
+	Transform t = get_transform();
+	if ((ABS(t.basis.get_axis(0).length() - 1.0) > 0.05 || ABS(t.basis.get_axis(1).length() - 1.0) > 0.05 || ABS(t.basis.get_axis(0).length() - 1.0) > 0.05)) {
+		if (!warning.empty())
+			warning += "\n\n";
+
+		warning += TTR("Size changes to SoftBody will be overriden by the physics engine when running.\nChange the size in children collision shapes instead.");
+	}
+
+	return warning;
+}
+
+void SoftBody::_draw_soft_mesh() {
+	if (get_mesh().is_null())
+		return;
+
+	if (!visual_server_handler.is_ready()) {
+
+		visual_server_handler.prepare(get_mesh()->get_rid(), 0);
+
+		/// Necessary in order to render the mesh correctly (Soft body nodes are in global space)
+		simulation_started = true;
+		call_deferred("set_as_toplevel", true);
+		call_deferred("set_transform", Transform());
+	}
+
+	visual_server_handler.open();
+	PhysicsServer::get_singleton()->soft_body_update_visual_server(physics_rid, &visual_server_handler);
+	visual_server_handler.close();
+
+	visual_server_handler.commit_changes();
+}
+
+void SoftBody::update_physics_server() {
+
+	if (Engine::get_singleton()->is_editor_hint())
+		return;
+
+	if (get_mesh().is_valid()) {
+
+		become_mesh_owner();
+		PhysicsServer::get_singleton()->soft_body_set_mesh(physics_rid, get_mesh());
+		VS::get_singleton()->connect("frame_pre_draw", this, "_draw_soft_mesh");
+	} else {
+
+		PhysicsServer::get_singleton()->soft_body_set_mesh(physics_rid, NULL);
+		VS::get_singleton()->disconnect("frame_pre_draw", this, "_draw_soft_mesh");
+	}
+}
+
+void SoftBody::become_mesh_owner() {
+	if (mesh.is_null())
+		return;
+
+	if (!mesh_owner) {
+		mesh_owner = true;
+
+		ERR_FAIL_COND(!mesh->get_surface_count());
+
+		// Get current mesh array and create new mesh array with necessary flag for softbody
+		Array surface_arrays = mesh->surface_get_arrays(0);
+		Array surface_blend_arrays = mesh->surface_get_blend_shape_arrays(0);
+		uint32_t surface_format = mesh->surface_get_format(0);
+
+		surface_format &= ~(Mesh::ARRAY_COMPRESS_VERTEX | Mesh::ARRAY_COMPRESS_NORMAL);
+		surface_format |= Mesh::ARRAY_FLAG_USE_DYNAMIC_UPDATE;
+
+		Ref<ArrayMesh> soft_mesh;
+		soft_mesh.instance();
+		soft_mesh->add_surface_from_arrays(Mesh::PRIMITIVE_TRIANGLES, surface_arrays, surface_blend_arrays, surface_format);
+
+		set_mesh(soft_mesh);
+
+		Vector<Ref<Material> > copy_materials;
+		copy_materials.append_array(materials);
+		for (int i = copy_materials.size() - 1; 0 <= i; --i) {
+			set_surface_material(i, copy_materials[i]);
+		}
+	}
+}
+
+void SoftBody::set_collision_mask(uint32_t p_mask) {
+	collision_mask = p_mask;
+	PhysicsServer::get_singleton()->soft_body_set_collision_mask(physics_rid, p_mask);
+}
+
+uint32_t SoftBody::get_collision_mask() const {
+	return collision_mask;
+}
+void SoftBody::set_collision_layer(uint32_t p_layer) {
+	collision_layer = p_layer;
+	PhysicsServer::get_singleton()->soft_body_set_collision_layer(physics_rid, p_layer);
+}
+
+uint32_t SoftBody::get_collision_layer() const {
+	return collision_layer;
+}
+
+void SoftBody::set_collision_mask_bit(int p_bit, bool p_value) {
+	uint32_t mask = get_collision_mask();
+	if (p_value)
+		mask |= 1 << p_bit;
+	else
+		mask &= ~(1 << p_bit);
+	set_collision_mask(mask);
+}
+
+bool SoftBody::get_collision_mask_bit(int p_bit) const {
+	return get_collision_mask() & (1 << p_bit);
+}
+
+void SoftBody::set_collision_layer_bit(int p_bit, bool p_value) {
+	uint32_t layer = get_collision_layer();
+	if (p_value)
+		layer |= 1 << p_bit;
+	else
+		layer &= ~(1 << p_bit);
+	set_collision_layer(layer);
+}
+
+bool SoftBody::get_collision_layer_bit(int p_bit) const {
+	return get_collision_layer() & (1 << p_bit);
+}
+
+void SoftBody::set_parent_collision_ignore(const NodePath &p_parent_collision_ignore) {
+	parent_collision_ignore = p_parent_collision_ignore;
+}
+
+const NodePath &SoftBody::get_parent_collision_ignore() const {
+	return parent_collision_ignore;
+}
+
+void SoftBody::set_pinned_points_indices(PoolVector<SoftBody::PinnedPoint> p_pinned_points_indices) {
+	pinned_points_indices = p_pinned_points_indices;
+	PoolVector<PinnedPoint>::Read w = pinned_points_indices.read();
+	for (int i = pinned_points_indices.size() - 1; 0 <= i; --i) {
+		pin_point(p_pinned_points_indices[i].point_index, true);
+	}
+}
+
+PoolVector<SoftBody::PinnedPoint> SoftBody::get_pinned_points_indices() {
+	return pinned_points_indices;
+}
+
+void SoftBody::add_collision_exception_with(Node *p_node) {
+	ERR_FAIL_NULL(p_node);
+	CollisionObject *collision_object = Object::cast_to<CollisionObject>(p_node);
+	if (!collision_object) {
+		ERR_EXPLAIN("Collision exception only works between two CollisionObject");
+	}
+	ERR_FAIL_COND(!collision_object);
+	PhysicsServer::get_singleton()->soft_body_add_collision_exception(physics_rid, collision_object->get_rid());
+}
+
+void SoftBody::remove_collision_exception_with(Node *p_node) {
+	ERR_FAIL_NULL(p_node);
+	CollisionObject *collision_object = Object::cast_to<CollisionObject>(p_node);
+	if (!collision_object) {
+		ERR_EXPLAIN("Collision exception only works between two CollisionObject");
+	}
+	ERR_FAIL_COND(!collision_object);
+	PhysicsServer::get_singleton()->soft_body_remove_collision_exception(physics_rid, collision_object->get_rid());
+}
+
+int SoftBody::get_simulation_precision() {
+	return PhysicsServer::get_singleton()->soft_body_get_simulation_precision(physics_rid);
+}
+
+void SoftBody::set_simulation_precision(int p_simulation_precision) {
+	PhysicsServer::get_singleton()->soft_body_set_simulation_precision(physics_rid, p_simulation_precision);
+}
+
+real_t SoftBody::get_total_mass() {
+	return PhysicsServer::get_singleton()->soft_body_get_total_mass(physics_rid);
+}
+
+void SoftBody::set_total_mass(real_t p_total_mass) {
+	PhysicsServer::get_singleton()->soft_body_set_total_mass(physics_rid, p_total_mass);
+}
+
+void SoftBody::set_linear_stiffness(real_t p_linear_stiffness) {
+	PhysicsServer::get_singleton()->soft_body_set_linear_stiffness(physics_rid, p_linear_stiffness);
+}
+
+real_t SoftBody::get_linear_stiffness() {
+	return PhysicsServer::get_singleton()->soft_body_get_linear_stiffness(physics_rid);
+}
+
+void SoftBody::set_areaAngular_stiffness(real_t p_areaAngular_stiffness) {
+	PhysicsServer::get_singleton()->soft_body_set_areaAngular_stiffness(physics_rid, p_areaAngular_stiffness);
+}
+
+real_t SoftBody::get_areaAngular_stiffness() {
+	return PhysicsServer::get_singleton()->soft_body_get_areaAngular_stiffness(physics_rid);
+}
+
+void SoftBody::set_volume_stiffness(real_t p_volume_stiffness) {
+	PhysicsServer::get_singleton()->soft_body_set_volume_stiffness(physics_rid, p_volume_stiffness);
+}
+
+real_t SoftBody::get_volume_stiffness() {
+	return PhysicsServer::get_singleton()->soft_body_get_volume_stiffness(physics_rid);
+}
+
+real_t SoftBody::get_pressure_coefficient() {
+	return PhysicsServer::get_singleton()->soft_body_get_pressure_coefficient(physics_rid);
+}
+
+void SoftBody::set_pose_matching_coefficient(real_t p_pose_matching_coefficient) {
+	PhysicsServer::get_singleton()->soft_body_set_pose_matching_coefficient(physics_rid, p_pose_matching_coefficient);
+}
+
+real_t SoftBody::get_pose_matching_coefficient() {
+	return PhysicsServer::get_singleton()->soft_body_get_pose_matching_coefficient(physics_rid);
+}
+
+void SoftBody::set_pressure_coefficient(real_t p_pressure_coefficient) {
+	PhysicsServer::get_singleton()->soft_body_set_pressure_coefficient(physics_rid, p_pressure_coefficient);
+}
+
+real_t SoftBody::get_damping_coefficient() {
+	return PhysicsServer::get_singleton()->soft_body_get_damping_coefficient(physics_rid);
+}
+
+void SoftBody::set_damping_coefficient(real_t p_damping_coefficient) {
+	PhysicsServer::get_singleton()->soft_body_set_damping_coefficient(physics_rid, p_damping_coefficient);
+}
+
+real_t SoftBody::get_drag_coefficient() {
+	return PhysicsServer::get_singleton()->soft_body_get_drag_coefficient(physics_rid);
+}
+
+void SoftBody::set_drag_coefficient(real_t p_drag_coefficient) {
+	PhysicsServer::get_singleton()->soft_body_set_drag_coefficient(physics_rid, p_drag_coefficient);
+}
+
+Vector3 SoftBody::get_point_transform(int p_point_index) {
+	return PhysicsServer::get_singleton()->soft_body_get_point_global_position(physics_rid, p_point_index);
+}
+
+void SoftBody::pin_point_toggle(int p_point_index) {
+	pin_point(p_point_index, !(-1 != _has_pinned_point(p_point_index)));
+}
+
+void SoftBody::pin_point(int p_point_index, bool pin, const NodePath &p_spatial_attachment_path) {
+	_pin_point_on_physics_server(p_point_index, pin);
+	if (pin) {
+		_add_pinned_point(p_point_index, p_spatial_attachment_path);
+	} else {
+		_remove_pinned_point(p_point_index);
+	}
+}
+
+bool SoftBody::is_point_pinned(int p_point_index) const {
+	return -1 != _has_pinned_point(p_point_index);
+}
+
+void SoftBody::set_ray_pickable(bool p_ray_pickable) {
+
+	ray_pickable = p_ray_pickable;
+	_update_pickable();
+}
+
+bool SoftBody::is_ray_pickable() const {
+
+	return ray_pickable;
+}
+
+SoftBody::SoftBody() :
+		MeshInstance(),
+		physics_rid(PhysicsServer::get_singleton()->soft_body_create()),
+		mesh_owner(false),
+		collision_mask(1),
+		collision_layer(1),
+		simulation_started(false),
+		pinned_points_cache_dirty(true) {
+
+	PhysicsServer::get_singleton()->body_attach_object_instance_id(physics_rid, get_instance_id());
+}
+
+SoftBody::~SoftBody() {
+}
+
+void SoftBody::reset_softbody_pin() {
+	PhysicsServer::get_singleton()->soft_body_remove_all_pinned_points(physics_rid);
+	PoolVector<PinnedPoint>::Read pps = pinned_points_indices.read();
+	for (int i = pinned_points_indices.size() - 1; 0 < i; --i) {
+		PhysicsServer::get_singleton()->soft_body_pin_point(physics_rid, pps[i].point_index, true);
+	}
+}
+
+void SoftBody::_update_cache_pin_points_datas() {
+	if (pinned_points_cache_dirty) {
+		pinned_points_cache_dirty = false;
+
+		PoolVector<PinnedPoint>::Write w = pinned_points_indices.write();
+		for (int i = pinned_points_indices.size() - 1; 0 <= i; --i) {
+
+			if (!w[i].spatial_attachment_path.is_empty()) {
+				w[i].spatial_attachment = Object::cast_to<Spatial>(get_node(w[i].spatial_attachment_path));
+				if (w[i].spatial_attachment) {
+
+					Transform point_global_transform(get_global_transform());
+					point_global_transform.translate(PhysicsServer::get_singleton()->soft_body_get_point_offset(physics_rid, w[i].point_index));
+
+					// Local transform relative to spatial attachment node
+					w[i].vertex_offset_transform = w[i].spatial_attachment->get_global_transform().affine_inverse() * point_global_transform;
+					continue;
+				} else {
+					ERR_PRINTS("The node with path: " + String(w[i].spatial_attachment_path) + " was not found or is not a spatial node.");
+				}
+			}
+			// Local transform relative to Soft body
+			w[i].vertex_offset_transform.origin = PhysicsServer::get_singleton()->soft_body_get_point_offset(physics_rid, w[i].point_index);
+			w[i].vertex_offset_transform.basis = Basis();
+		}
+	}
+}
+
+void SoftBody::_pin_point_on_physics_server(int p_point_index, bool pin) {
+	PhysicsServer::get_singleton()->soft_body_pin_point(physics_rid, p_point_index, pin);
+}
+
+void SoftBody::_add_pinned_point(int p_point_index, const NodePath &p_spatial_attachment_path) {
+	SoftBody::PinnedPoint *pinned_point;
+	if (-1 == _get_pinned_point(p_point_index, pinned_point)) {
+		// Create new
+		PinnedPoint pp;
+		pp.point_index = p_point_index;
+		pp.spatial_attachment_path = p_spatial_attachment_path;
+		pinned_points_indices.push_back(pp);
+	} else {
+		// Update
+		pinned_point->point_index = p_point_index;
+		pinned_point->spatial_attachment_path = p_spatial_attachment_path;
+	}
+}
+
+void SoftBody::_remove_pinned_point(int p_point_index) {
+	const int id(_has_pinned_point(p_point_index));
+	if (-1 != id) {
+		pinned_points_indices.remove(id);
+	}
+}
+
+int SoftBody::_get_pinned_point(int p_point_index, SoftBody::PinnedPoint *&r_point) const {
+	const int id = _has_pinned_point(p_point_index);
+	if (-1 == id) {
+		r_point = NULL;
+		return -1;
+	} else {
+		r_point = const_cast<SoftBody::PinnedPoint *>(&pinned_points_indices.read()[id]);
+		return id;
+	}
+}
+
+int SoftBody::_has_pinned_point(int p_point_index) const {
+	PoolVector<PinnedPoint>::Read r = pinned_points_indices.read();
+	for (int i = pinned_points_indices.size() - 1; 0 <= i; --i) {
+		if (p_point_index == r[i].point_index) {
+			return i;
+		}
+	}
+	return -1;
+}
diff --git a/scene/3d/soft_body.h b/scene/3d/soft_body.h
new file mode 100644
index 0000000000..f44f337698
--- /dev/null
+++ b/scene/3d/soft_body.h
@@ -0,0 +1,197 @@
+/*************************************************************************/
+/*  soft_physics_body.h                                                  */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 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_PHYSICS_BODY_H
+#define SOFT_PHYSICS_BODY_H
+
+#include "scene/3d/mesh_instance.h"
+
+class SoftBody;
+
+class SoftBodyVisualServerHandler {
+
+	friend class SoftBody;
+
+	RID mesh;
+	int surface;
+	PoolVector<uint8_t> buffer;
+	uint32_t stride;
+	uint32_t offset_vertices;
+	uint32_t offset_normal;
+
+	PoolVector<uint8_t>::Write write_buffer;
+
+private:
+	SoftBodyVisualServerHandler();
+	bool is_ready() { return mesh.is_valid(); }
+	void prepare(RID p_mesh_rid, int p_surface);
+	void clear();
+	void open();
+	void close();
+	void commit_changes();
+
+public:
+	void set_vertex(int p_vertex_id, const void *p_vector3);
+	void set_normal(int p_vertex_id, const void *p_vector3);
+	void set_aabb(const AABB &p_aabb);
+};
+
+class SoftBody : public MeshInstance {
+	GDCLASS(SoftBody, MeshInstance);
+
+public:
+	struct PinnedPoint {
+		int point_index;
+		NodePath spatial_attachment_path;
+		Spatial *spatial_attachment; // Cache
+		/// This is the offset from the soft body to point or attachment to point
+		/// Depend if the spatial_attachment_node is NULL or not
+		Transform vertex_offset_transform; // Cache
+
+		PinnedPoint();
+		PinnedPoint(const PinnedPoint &obj_tocopy);
+	};
+
+private:
+	SoftBodyVisualServerHandler visual_server_handler;
+
+	RID physics_rid;
+
+	bool mesh_owner;
+	uint32_t collision_mask;
+	uint32_t collision_layer;
+	NodePath parent_collision_ignore;
+	PoolVector<PinnedPoint> pinned_points_indices;
+	bool simulation_started;
+	bool pinned_points_cache_dirty;
+
+	Ref<ArrayMesh> debug_mesh_cache;
+	class MeshInstance *debug_mesh;
+
+	bool capture_input_on_drag;
+	bool ray_pickable;
+
+	void _update_pickable();
+
+protected:
+	bool _set(const StringName &p_name, const Variant &p_value);
+	bool _get(const StringName &p_name, Variant &r_ret) const;
+	void _get_property_list(List<PropertyInfo> *p_list) const;
+
+	bool _set_property_pinned_points_indices(const Array &p_indices);
+	bool _set_property_pinned_points_attachment(int p_item, const String &p_what, const Variant &p_value);
+	bool _get_property_pinned_points(int p_item, const String &p_what, Variant &r_ret) const;
+
+	virtual void _changed_callback(Object *p_changed, const char *p_prop);
+
+	void _notification(int p_what);
+	static void _bind_methods();
+
+	virtual String get_configuration_warning() const;
+
+protected:
+	void _draw_soft_mesh();
+
+public:
+	void update_physics_server();
+	void become_mesh_owner();
+
+	void set_collision_mask(uint32_t p_mask);
+	uint32_t get_collision_mask() const;
+
+	void set_collision_layer(uint32_t p_layer);
+	uint32_t get_collision_layer() const;
+
+	void set_collision_mask_bit(int p_bit, bool p_value);
+	bool get_collision_mask_bit(int p_bit) const;
+
+	void set_collision_layer_bit(int p_bit, bool p_value);
+	bool get_collision_layer_bit(int p_bit) const;
+
+	void set_parent_collision_ignore(const NodePath &p_parent_collision_ignore);
+	const NodePath &get_parent_collision_ignore() const;
+
+	void set_pinned_points_indices(PoolVector<PinnedPoint> p_pinned_points_indices);
+	PoolVector<PinnedPoint> get_pinned_points_indices();
+
+	void set_simulation_precision(int p_simulation_precision);
+	int get_simulation_precision();
+
+	void set_total_mass(real_t p_total_mass);
+	real_t get_total_mass();
+
+	void set_linear_stiffness(real_t p_linear_stiffness);
+	real_t get_linear_stiffness();
+
+	void set_areaAngular_stiffness(real_t p_areaAngular_stiffness);
+	real_t get_areaAngular_stiffness();
+
+	void set_volume_stiffness(real_t p_volume_stiffness);
+	real_t get_volume_stiffness();
+
+	void set_pressure_coefficient(real_t p_pressure_coefficient);
+	real_t get_pressure_coefficient();
+
+	void set_pose_matching_coefficient(real_t p_pose_matching_coefficient);
+	real_t get_pose_matching_coefficient();
+
+	void set_damping_coefficient(real_t p_damping_coefficient);
+	real_t get_damping_coefficient();
+
+	void set_drag_coefficient(real_t p_drag_coefficient);
+	real_t get_drag_coefficient();
+
+	void add_collision_exception_with(Node *p_node);
+	void remove_collision_exception_with(Node *p_node);
+
+	Vector3 get_point_transform(int p_point_index);
+
+	void pin_point_toggle(int p_point_index);
+	void pin_point(int p_point_index, bool pin, const NodePath &p_spatial_attachment_path = NodePath());
+	bool is_point_pinned(int p_point_index) const;
+
+	void set_ray_pickable(bool p_ray_pickable);
+	bool is_ray_pickable() const;
+
+	SoftBody();
+	~SoftBody();
+
+private:
+	void reset_softbody_pin();
+	void _update_cache_pin_points_datas();
+	void _pin_point_on_physics_server(int p_point_index, bool pin);
+	void _add_pinned_point(int p_point_index, const NodePath &p_spatial_attachment_path);
+	void _remove_pinned_point(int p_point_index);
+	int _get_pinned_point(int p_point_index, PinnedPoint *&r_point) const;
+	int _has_pinned_point(int p_point_index) const;
+};
+
+#endif // SOFT_PHYSICS_BODY_H
diff --git a/scene/register_scene_types.cpp b/scene/register_scene_types.cpp
index ee94b64c20..0d69c037fe 100644
--- a/scene/register_scene_types.cpp
+++ b/scene/register_scene_types.cpp
@@ -199,6 +199,7 @@
 #include "scene/3d/remote_transform.h"
 #include "scene/3d/room_instance.h"
 #include "scene/3d/skeleton.h"
+#include "scene/3d/soft_body.h"
 #include "scene/3d/sprite_3d.h"
 #include "scene/3d/vehicle_body.h"
 #include "scene/3d/visibility_notifier.h"
@@ -428,6 +429,7 @@ void register_scene_types() {
 	ClassDB::register_class<KinematicCollision>();
 	ClassDB::register_class<KinematicBody>();
 	ClassDB::register_class<PhysicalBone>();
+	ClassDB::register_class<SoftBody>();
 
 	ClassDB::register_class<VehicleBody>();
 	ClassDB::register_class<VehicleWheel>();
diff --git a/scene/resources/mesh.cpp b/scene/resources/mesh.cpp
index b0620d3363..fe87dcdd2c 100644
--- a/scene/resources/mesh.cpp
+++ b/scene/resources/mesh.cpp
@@ -40,7 +40,6 @@
 void Mesh::_clear_triangle_mesh() const {
 
 	triangle_mesh.unref();
-	;
 }
 
 Ref<TriangleMesh> Mesh::generate_triangle_mesh() const {
@@ -110,6 +109,54 @@ Ref<TriangleMesh> Mesh::generate_triangle_mesh() const {
 	return triangle_mesh;
 }
 
+void Mesh::generate_debug_mesh_lines(Vector<Vector3> &r_lines) {
+
+	Ref<TriangleMesh> tm = generate_triangle_mesh();
+	if (tm.is_null())
+		return;
+
+	PoolVector<int> triangle_indices;
+	tm->get_indices(&triangle_indices);
+	const int triangles_num = tm->get_triangles().size();
+	PoolVector<Vector3> vertices = tm->get_vertices();
+
+	r_lines.resize(tm->get_triangles().size() * 6); // 3 lines x 2 points each line
+
+	PoolVector<int>::Read ind_r = triangle_indices.read();
+	PoolVector<Vector3>::Read ver_r = vertices.read();
+	for (int j = 0, x = 0, i = 0; i < triangles_num; j += 6, x += 3, ++i) {
+		// Triangle line 1
+		r_lines[j + 0] = ver_r[ind_r[x + 0]];
+		r_lines[j + 1] = ver_r[ind_r[x + 1]];
+
+		// Triangle line 2
+		r_lines[j + 2] = ver_r[ind_r[x + 1]];
+		r_lines[j + 3] = ver_r[ind_r[x + 2]];
+
+		// Triangle line 3
+		r_lines[j + 4] = ver_r[ind_r[x + 2]];
+		r_lines[j + 5] = ver_r[ind_r[x + 0]];
+	}
+}
+void Mesh::generate_debug_mesh_indices(Vector<Vector3> &r_points) {
+	Ref<TriangleMesh> tm = generate_triangle_mesh();
+	if (tm.is_null())
+		return;
+
+	PoolVector<Vector3> vertices = tm->get_vertices();
+
+	int vertices_size = vertices.size();
+	r_points.resize(vertices_size);
+	for (int i = 0; i < vertices_size; ++i) {
+		r_points[i] = vertices[i];
+	}
+}
+
+bool Mesh::surface_is_softbody_friendly(int p_idx) const {
+	const uint32_t surface_format = surface_get_format(p_idx);
+	return (surface_format & Mesh::ARRAY_FLAG_USE_DYNAMIC_UPDATE && (!(surface_format & Mesh::ARRAY_COMPRESS_VERTEX)) && (!(surface_format & Mesh::ARRAY_COMPRESS_NORMAL)));
+}
+
 PoolVector<Face3> Mesh::get_faces() const {
 
 	Ref<TriangleMesh> tm = generate_triangle_mesh();
@@ -484,6 +531,10 @@ void Mesh::_bind_methods() {
 	BIND_ENUM_CONSTANT(ARRAY_MAX);
 }
 
+void Mesh::clear_cache() {
+	_clear_triangle_mesh();
+}
+
 Mesh::Mesh() {
 }
 
diff --git a/scene/resources/mesh.h b/scene/resources/mesh.h
index a3fb068569..2127eaae4c 100644
--- a/scene/resources/mesh.h
+++ b/scene/resources/mesh.h
@@ -123,6 +123,7 @@ public:
 	virtual int get_surface_count() const = 0;
 	virtual int surface_get_array_len(int p_idx) const = 0;
 	virtual int surface_get_array_index_len(int p_idx) const = 0;
+	virtual bool surface_is_softbody_friendly(int p_idx) const;
 	virtual Array surface_get_arrays(int p_surface) const = 0;
 	virtual Array surface_get_blend_shape_arrays(int p_surface) const = 0;
 	virtual uint32_t surface_get_format(int p_idx) const = 0;
@@ -133,6 +134,8 @@ public:
 
 	PoolVector<Face3> get_faces() const;
 	Ref<TriangleMesh> generate_triangle_mesh() const;
+	void generate_debug_mesh_lines(Vector<Vector3> &r_lines);
+	void generate_debug_mesh_indices(Vector<Vector3> &r_points);
 
 	Ref<Shape> create_trimesh_shape() const;
 	Ref<Shape> create_convex_shape() const;
@@ -143,6 +146,7 @@ public:
 
 	void set_lightmap_size_hint(const Vector2 &p_size);
 	Size2 get_lightmap_size_hint() const;
+	void clear_cache();
 
 	Mesh();
 };
diff --git a/scene/resources/surface_tool.cpp b/scene/resources/surface_tool.cpp
index 5a42873d79..3df9ab26a4 100644
--- a/scene/resources/surface_tool.cpp
+++ b/scene/resources/surface_tool.cpp
@@ -421,6 +421,7 @@ Ref<ArrayMesh> SurfaceTool::commit(const Ref<ArrayMesh> &p_existing, uint32_t p_
 	Array a = commit_to_arrays();
 
 	mesh->add_surface_from_arrays(primitive, a, Array(), p_flags);
+
 	if (material.is_valid())
 		mesh->surface_set_material(surface, material);
 
diff --git a/servers/physics/physics_server_sw.h b/servers/physics/physics_server_sw.h
index d8c73abb16..7e9700d026 100644
--- a/servers/physics/physics_server_sw.h
+++ b/servers/physics/physics_server_sw.h
@@ -234,6 +234,72 @@ public:
 	// this function only works on physics process, errors and returns null otherwise
 	virtual PhysicsDirectBodyState *body_get_direct_state(RID p_body);
 
+	/* SOFT BODY */
+
+	virtual RID soft_body_create(bool p_init_sleeping = false) { return RID(); }
+
+	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 { return RID(); }
+
+	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 { return 0; }
+
+	virtual void soft_body_set_collision_mask(RID p_body, uint32_t p_mask) {}
+	virtual uint32_t soft_body_get_collision_mask(RID p_body) const { return 0; }
+
+	virtual void soft_body_add_collision_exception(RID p_body, RID p_body_b) {}
+	virtual void soft_body_remove_collision_exception(RID p_body, RID p_body_b) {}
+	virtual void soft_body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) {}
+
+	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 { return Variant(); }
+
+	virtual void soft_body_set_transform(RID p_body, const Transform &p_transform) {}
+	virtual Vector3 soft_body_get_vertex_position(RID p_body, int vertex_index) const { return Vector3(); }
+
+	virtual void soft_body_set_ray_pickable(RID p_body, bool p_enable) {}
+	virtual bool soft_body_is_ray_pickable(RID p_body) const { return false; }
+
+	virtual void soft_body_set_simulation_precision(RID p_body, int p_simulation_precision) {}
+	virtual int soft_body_get_simulation_precision(RID p_body) { return 0; }
+
+	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) { return 0.; }
+
+	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) { return 0.; }
+
+	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) { return 0.; }
+
+	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) { return 0.; }
+
+	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) { return 0.; }
+
+	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) { return 0.; }
+
+	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) { return 0.; }
+
+	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) { return 0.; }
+
+	virtual void soft_body_set_mesh(RID p_body, const REF &p_mesh) {}
+
+	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) { return Vector3(); }
+
+	virtual Vector3 soft_body_get_point_offset(RID p_body, int p_point_index) const { return Vector3(); }
+
+	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) { return 0; }
+
 	/* JOINT API */
 
 	virtual RID joint_create_pin(RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B);
diff --git a/servers/physics_server.h b/servers/physics_server.h
index 91e7f58319..497d23c555 100644
--- a/servers/physics_server.h
+++ b/servers/physics_server.h
@@ -490,6 +490,72 @@ public:
 
 	virtual bool body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result = NULL) = 0;
 
+	/* SOFT BODY */
+
+	virtual RID soft_body_create(bool p_init_sleeping = false) = 0;
+
+	virtual void soft_body_update_visual_server(RID p_body, class SoftBodyVisualServerHandler *p_visual_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 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;
+
+	virtual void soft_body_set_collision_mask(RID p_body, uint32_t p_mask) = 0;
+	virtual uint32_t soft_body_get_collision_mask(RID p_body) const = 0;
+
+	virtual void soft_body_add_collision_exception(RID p_body, RID p_body_b) = 0;
+	virtual void soft_body_remove_collision_exception(RID p_body, RID p_body_b) = 0;
+	virtual void soft_body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) = 0;
+
+	virtual void soft_body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) = 0;
+	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;
+	virtual bool soft_body_is_ray_pickable(RID p_body) const = 0;
+
+	virtual void soft_body_set_simulation_precision(RID p_body, int p_simulation_precision) = 0;
+	virtual int soft_body_get_simulation_precision(RID p_body) = 0;
+
+	virtual void soft_body_set_total_mass(RID p_body, real_t p_total_mass) = 0;
+	virtual real_t soft_body_get_total_mass(RID p_body) = 0;
+
+	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) = 0;
+
+	virtual void soft_body_set_areaAngular_stiffness(RID p_body, real_t p_stiffness) = 0;
+	virtual real_t soft_body_get_areaAngular_stiffness(RID p_body) = 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) = 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) = 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) = 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) = 0;
+
+	virtual void soft_body_set_drag_coefficient(RID p_body, real_t p_drag_coefficient) = 0;
+	virtual real_t soft_body_get_drag_coefficient(RID p_body) = 0;
+
+	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) = 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) = 0;
+
 	/* JOINT API */
 
 	enum JointType {
diff --git a/servers/visual/rasterizer.h b/servers/visual/rasterizer.h
index 731a61890c..a8f4377ce7 100644
--- a/servers/visual/rasterizer.h
+++ b/servers/visual/rasterizer.h
@@ -280,6 +280,7 @@ public:
 	virtual AABB mesh_get_custom_aabb(RID p_mesh) const = 0;
 
 	virtual AABB mesh_get_aabb(RID p_mesh, RID p_skeleton) const = 0;
+
 	virtual void mesh_clear(RID p_mesh) = 0;
 
 	/* MULTIMESH API */
diff --git a/servers/visual_server.cpp b/servers/visual_server.cpp
index 143e56b848..8e21341cee 100644
--- a/servers/visual_server.cpp
+++ b/servers/visual_server.cpp
@@ -795,6 +795,140 @@ Error VisualServer::_surface_set_data(Array p_arrays, uint32_t p_format, uint32_
 	return OK;
 }
 
+uint32_t VisualServer::mesh_surface_get_format_offset(uint32_t p_format, int p_vertex_len, int p_index_len, int p_array_index) const {
+	uint32_t offsets[ARRAY_MAX];
+	mesh_surface_make_offsets_from_format(p_format, p_vertex_len, p_index_len, offsets);
+	return offsets[p_array_index];
+}
+
+uint32_t VisualServer::mesh_surface_get_format_stride(uint32_t p_format, int p_vertex_len, int p_index_len) const {
+	uint32_t offsets[ARRAY_MAX];
+	return mesh_surface_make_offsets_from_format(p_format, p_vertex_len, p_index_len, offsets);
+}
+
+uint32_t VisualServer::mesh_surface_make_offsets_from_format(uint32_t p_format, int p_vertex_len, int p_index_len, uint32_t *r_offsets) const {
+
+	int total_elem_size = 0;
+
+	for (int i = 0; i < VS::ARRAY_MAX; i++) {
+
+		r_offsets[i] = 0; //reset
+
+		if (!(p_format & (1 << i))) // no array
+			continue;
+
+		int elem_size = 0;
+
+		switch (i) {
+
+			case VS::ARRAY_VERTEX: {
+
+				if (p_format & ARRAY_FLAG_USE_2D_VERTICES) {
+					elem_size = 2;
+				} else {
+					elem_size = 3;
+				}
+
+				if (p_format & ARRAY_COMPRESS_VERTEX) {
+					elem_size *= sizeof(int16_t);
+				} else {
+					elem_size *= sizeof(float);
+				}
+
+				if (elem_size == 6) {
+					elem_size = 8;
+				}
+
+			} break;
+			case VS::ARRAY_NORMAL: {
+
+				if (p_format & ARRAY_COMPRESS_NORMAL) {
+					elem_size = sizeof(uint32_t);
+				} else {
+					elem_size = sizeof(float) * 3;
+				}
+
+			} break;
+
+			case VS::ARRAY_TANGENT: {
+				if (p_format & ARRAY_COMPRESS_TANGENT) {
+					elem_size = sizeof(uint32_t);
+				} else {
+					elem_size = sizeof(float) * 4;
+				}
+
+			} break;
+			case VS::ARRAY_COLOR: {
+
+				if (p_format & ARRAY_COMPRESS_COLOR) {
+					elem_size = sizeof(uint32_t);
+				} else {
+					elem_size = sizeof(float) * 4;
+				}
+			} break;
+			case VS::ARRAY_TEX_UV: {
+				if (p_format & ARRAY_COMPRESS_TEX_UV) {
+					elem_size = sizeof(uint32_t);
+				} else {
+					elem_size = sizeof(float) * 2;
+				}
+
+			} break;
+
+			case VS::ARRAY_TEX_UV2: {
+				if (p_format & ARRAY_COMPRESS_TEX_UV2) {
+					elem_size = sizeof(uint32_t);
+				} else {
+					elem_size = sizeof(float) * 2;
+				}
+
+			} break;
+			case VS::ARRAY_WEIGHTS: {
+
+				if (p_format & ARRAY_COMPRESS_WEIGHTS) {
+					elem_size = sizeof(uint16_t) * 4;
+				} else {
+					elem_size = sizeof(float) * 4;
+				}
+
+			} break;
+			case VS::ARRAY_BONES: {
+
+				if (p_format & ARRAY_FLAG_USE_16_BIT_BONES) {
+					elem_size = sizeof(uint16_t) * 4;
+				} else {
+					elem_size = sizeof(uint32_t);
+				}
+
+			} break;
+			case VS::ARRAY_INDEX: {
+
+				if (p_index_len <= 0) {
+					ERR_PRINT("index_array_len==NO_INDEX_ARRAY");
+					break;
+				}
+				/* determine whether using 16 or 32 bits indices */
+				if (p_vertex_len >= (1 << 16)) {
+
+					elem_size = 4;
+
+				} else {
+					elem_size = 2;
+				}
+				r_offsets[i] = elem_size;
+				continue;
+			} break;
+			default: {
+				ERR_FAIL_V(0);
+			}
+		}
+
+		r_offsets[i] = total_elem_size;
+		total_elem_size += elem_size;
+	}
+	return total_elem_size;
+}
+
 void VisualServer::mesh_add_surface_from_arrays(RID p_mesh, PrimitiveType p_primitive, const Array &p_arrays, const Array &p_blend_shapes, uint32_t p_compress_format) {
 
 	ERR_FAIL_INDEX(p_primitive, VS::PRIMITIVE_MAX);
@@ -1564,11 +1698,14 @@ void VisualServer::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("material_set_next_pass", "material", "next_material"), &VisualServer::material_set_next_pass);
 
 	ClassDB::bind_method(D_METHOD("mesh_create"), &VisualServer::mesh_create);
+	ClassDB::bind_method(D_METHOD("mesh_surface_get_format_offset", "format", "vertex_len", "index_len", "array_index"), &VisualServer::mesh_surface_get_format_offset);
+	ClassDB::bind_method(D_METHOD("mesh_surface_get_format_stride", "format", "vertex_len", "index_len"), &VisualServer::mesh_surface_get_format_stride);
 	ClassDB::bind_method(D_METHOD("mesh_add_surface_from_arrays", "mesh", "primtive", "arrays", "blend_shapes", "compress_format"), &VisualServer::mesh_add_surface_from_arrays, DEFVAL(Array()), DEFVAL(ARRAY_COMPRESS_DEFAULT));
 	ClassDB::bind_method(D_METHOD("mesh_set_blend_shape_count", "mesh", "amount"), &VisualServer::mesh_set_blend_shape_count);
 	ClassDB::bind_method(D_METHOD("mesh_get_blend_shape_count", "mesh"), &VisualServer::mesh_get_blend_shape_count);
 	ClassDB::bind_method(D_METHOD("mesh_set_blend_shape_mode", "mesh", "mode"), &VisualServer::mesh_set_blend_shape_mode);
 	ClassDB::bind_method(D_METHOD("mesh_get_blend_shape_mode", "mesh"), &VisualServer::mesh_get_blend_shape_mode);
+	ClassDB::bind_method(D_METHOD("mesh_surface_update_region", "mesh", "surface", "offset", "data"), &VisualServer::mesh_surface_update_region);
 	ClassDB::bind_method(D_METHOD("mesh_surface_set_material", "mesh", "surface", "material"), &VisualServer::mesh_surface_set_material);
 	ClassDB::bind_method(D_METHOD("mesh_surface_get_material", "mesh", "surface"), &VisualServer::mesh_surface_get_material);
 	ClassDB::bind_method(D_METHOD("mesh_surface_get_array_len", "mesh", "surface"), &VisualServer::mesh_surface_get_array_len);
diff --git a/servers/visual_server.h b/servers/visual_server.h
index a76cad24a2..367642b7d4 100644
--- a/servers/visual_server.h
+++ b/servers/visual_server.h
@@ -251,6 +251,10 @@ public:
 
 	virtual RID mesh_create() = 0;
 
+	virtual uint32_t mesh_surface_get_format_offset(uint32_t p_format, int p_vertex_len, int p_index_len, int p_array_index) const;
+	virtual uint32_t mesh_surface_get_format_stride(uint32_t p_format, int p_vertex_len, int p_index_len) const;
+	/// Returns stride
+	virtual uint32_t mesh_surface_make_offsets_from_format(uint32_t p_format, int p_vertex_len, int p_index_len, uint32_t *r_offsets) const;
 	virtual void mesh_add_surface_from_arrays(RID p_mesh, PrimitiveType p_primitive, const Array &p_arrays, const Array &p_blend_shapes = Array(), uint32_t p_compress_format = ARRAY_COMPRESS_DEFAULT);
 	virtual void mesh_add_surface(RID p_mesh, uint32_t p_format, PrimitiveType p_primitive, const PoolVector<uint8_t> &p_array, int p_vertex_count, const PoolVector<uint8_t> &p_index_array, int p_index_count, const AABB &p_aabb, const Vector<PoolVector<uint8_t> > &p_blend_shapes = Vector<PoolVector<uint8_t> >(), const Vector<AABB> &p_bone_aabbs = Vector<AABB>()) = 0;