godot/modules/bullet/soft_body_bullet.cpp
Andrea Catania 8d0d6d6921 - Enhanced the flush mechanism by flushing only needed thing.
- Flushing Areas before anything else.
- Make sure to correctly fetch gravity when the integrate_forces function is used
- Lazy reload body when layer and mask changes
- Shapes are reloaded just before the physics step starts.
- Improved some other parts of the code.
- Added override keyword
- Using LocalVector
2020-07-27 08:25:47 +02:00

468 lines
14 KiB
C++

/*************************************************************************/
/* soft_body_bullet.cpp */
/*************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/*************************************************************************/
/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */
/* Copyright (c) 2014-2020 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_bullet.h"
#include "bullet_types_converter.h"
#include "bullet_utilities.h"
#include "scene/3d/soft_body_3d.h"
#include "space_bullet.h"
SoftBodyBullet::SoftBodyBullet() :
CollisionObjectBullet(CollisionObjectBullet::TYPE_SOFT_BODY) {}
SoftBodyBullet::~SoftBodyBullet() {
}
void SoftBodyBullet::do_reload_body() {
if (space) {
space->remove_soft_body(this);
space->add_soft_body(this);
}
}
void SoftBodyBullet::set_space(SpaceBullet *p_space) {
if (space) {
isScratched = false;
space->unregister_collision_object(this);
space->remove_soft_body(this);
}
space = p_space;
if (space) {
space->register_collision_object(this);
reload_body();
}
}
void SoftBodyBullet::on_enter_area(AreaBullet *p_area) {}
void SoftBodyBullet::on_exit_area(AreaBullet *p_area) {}
void SoftBodyBullet::update_rendering_server(SoftBodyRenderingServerHandler *p_rendering_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_rendering_server_handler->set_vertex((*vs_indices)[x], vertex_position);
p_rendering_server_handler->set_normal((*vs_indices)[x], vertex_normal);
}
}
/// 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_rendering_server_handler->set_aabb(aabb);
}
void SoftBodyBullet::set_soft_mesh(const Ref<Mesh> &p_mesh) {
if (p_mesh.is_null()) {
soft_mesh.unref();
} else {
soft_mesh = p_mesh;
}
if (soft_mesh.is_null()) {
destroy_soft_body();
return;
}
Array arrays = soft_mesh->surface_get_arrays(0);
ERR_FAIL_COND(!(soft_mesh->surface_get_format(0) & RS::ARRAY_FORMAT_INDEX));
set_trimesh_body_shape(arrays[RS::ARRAY_INDEX], arrays[RS::ARRAY_VERTEX]);
}
void SoftBodyBullet::destroy_soft_body() {
if (!bt_soft_body) {
return;
}
if (space) {
/// Remove from world before deletion
space->remove_soft_body(this);
}
destroyBulletCollisionObject();
bt_soft_body = nullptr;
}
void SoftBodyBullet::set_soft_transform(const Transform &p_transform) {
reset_all_node_positions();
move_all_nodes(p_transform);
}
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::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::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_q = bt_soft_body->m_nodes[p_node_index].m_x;
bt_soft_body->m_nodes[p_node_index].m_x = p_global_position;
}
}
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::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);
Vector<Vector3> vertices(arrays[RS::ARRAY_VERTEX]);
if (0 <= p_node_index && vertices.size() > p_node_index) {
r_offset = vertices[p_node_index];
}
}
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_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);
}
}
btScalar SoftBodyBullet::get_node_mass(int node_index) const {
if (bt_soft_body) {
return bt_soft_body->getMass(node_index);
} else {
return -1 == search_node_pinned(node_index) ? 1 : 0;
}
}
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::reset_all_node_positions() {
if (soft_mesh.is_null()) {
return;
}
Array arrays = soft_mesh->surface_get_arrays(0);
Vector<Vector3> vs_vertices(arrays[RS::ARRAY_VERTEX]);
const Vector3 *vs_vertices_read = vs_vertices.ptr();
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);
}
}
void SoftBodyBullet::set_activation_state(bool p_active) {
if (p_active) {
bt_soft_body->setActivationState(ACTIVE_TAG);
} else {
bt_soft_body->setActivationState(WANTS_DEACTIVATION);
}
}
void SoftBodyBullet::set_total_mass(real_t p_val) {
if (0 >= p_val) {
p_val = 1;
}
total_mass = p_val;
if (bt_soft_body) {
bt_soft_body->setTotalMass(total_mass);
}
}
void SoftBodyBullet::set_linear_stiffness(real_t p_val) {
linear_stiffness = p_val;
if (bt_soft_body) {
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;
}
}
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;
}
}
void SoftBodyBullet::set_pressure_coefficient(real_t p_val) {
pressure_coefficient = p_val;
if (bt_soft_body) {
bt_soft_body->m_cfg.kPR = pressure_coefficient;
}
}
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) {
bt_soft_body->m_cfg.kDP = damping_coefficient;
}
}
void SoftBodyBullet::set_drag_coefficient(real_t p_val) {
drag_coefficient = p_val;
if (bt_soft_body) {
bt_soft_body->m_cfg.kDG = drag_coefficient;
}
}
void SoftBodyBullet::set_trimesh_body_shape(Vector<int> p_indices, Vector<Vector3> p_vertices) {
/// Assert the current soft body is destroyed
destroy_soft_body();
/// Parse visual server indices to physical indices.
/// Merge all overlapping vertices and create a map of physical vertices to visual server
{
/// 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;
{ // Map vertices
indices_table.resize(0);
int index = 0;
Map<Vector3, int> unique_vertices;
const int vs_vertices_size(p_vertices.size());
const Vector3 *p_vertices_read = p_vertices.ptr();
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());
LocalVector<btScalar> bt_vertices;
{ // Parse vertices to bullet
bt_vertices.resize(indices_map_size * 3);
const Vector3 *p_vertices_read = p_vertices.ptr();
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;
}
}
LocalVector<int> bt_triangles;
const int triangles_size(p_indices.size() / 3);
{ // Parse indices
bt_triangles.resize(triangles_size * 3);
const int *p_indices_read = p_indices.ptr();
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]];
}
}
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->m_worldInfo = nullptr; // 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::pin_node(int p_node_index) {
if (-1 == search_node_pinned(p_node_index)) {
pinned_nodes.push_back(p_node_index);
}
}
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;
}