1 files changed, 319 insertions, 155 deletions

diff --git a/modules/bullet/soft_body_bullet.cpp b/modules/bullet/soft_body_bullet.cpp
index 5c20eb73f1..1686a6e87e 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();
+
+	const 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.write[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.write[3 * i + 0] = p_vertices_read[indices_table[i][0]].x;
+				bt_vertices.write[3 * i + 1] = p_vertices_read[indices_table[i][0]].y;
+				bt_vertices.write[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.write[3 * i + 0] = vs_indices_to_physics_table[p_indices_read[3 * i + 2]];
+				bt_triangles.write[3 * i + 1] = vs_indices_to_physics_table[p_indices_read[3 * i + 1]];
+				bt_triangles.write[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;
 }