diff options
Diffstat (limited to 'servers/physics_3d/soft_body_3d_sw.cpp')
| -rw-r--r-- | servers/physics_3d/soft_body_3d_sw.cpp | 1221 |
1 files changed, 1221 insertions, 0 deletions
diff --git a/servers/physics_3d/soft_body_3d_sw.cpp b/servers/physics_3d/soft_body_3d_sw.cpp new file mode 100644 index 0000000000..f63a470cbe --- /dev/null +++ b/servers/physics_3d/soft_body_3d_sw.cpp @@ -0,0 +1,1221 @@ +/*************************************************************************/ +/* soft_body_3d_sw.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "soft_body_3d_sw.h" +#include "space_3d_sw.h" + +#include "core/math/geometry_3d.h" +#include "core/templates/map.h" + +// Based on Bullet soft body. + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +///btSoftBody implementation by Nathanael Presson + +SoftBody3DSW::SoftBody3DSW() : + CollisionObject3DSW(TYPE_SOFT_BODY), + active_list(this) { + _set_static(false); +} + +void SoftBody3DSW::_shapes_changed() { +} + +void SoftBody3DSW::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_variant) { + switch (p_state) { + case PhysicsServer3D::BODY_STATE_TRANSFORM: { + _set_transform(p_variant); + _set_inv_transform(get_transform().inverse()); + + apply_nodes_transform(get_transform()); + + } break; + case PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY: { + // Not supported. + ERR_FAIL_MSG("Linear velocity is not supported for Soft bodies."); + } break; + case PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY: { + ERR_FAIL_MSG("Angular velocity is not supported for Soft bodies."); + } break; + case PhysicsServer3D::BODY_STATE_SLEEPING: { + ERR_FAIL_MSG("Sleeping state is not supported for Soft bodies."); + } break; + case PhysicsServer3D::BODY_STATE_CAN_SLEEP: { + ERR_FAIL_MSG("Sleeping state is not supported for Soft bodies."); + } break; + } +} + +Variant SoftBody3DSW::get_state(PhysicsServer3D::BodyState p_state) const { + switch (p_state) { + case PhysicsServer3D::BODY_STATE_TRANSFORM: { + return get_transform(); + } break; + case PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY: { + ERR_FAIL_V_MSG(Vector3(), "Linear velocity is not supported for Soft bodies."); + } break; + case PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY: { + ERR_FAIL_V_MSG(Vector3(), "Angular velocity is not supported for Soft bodies."); + return Vector3(); + } break; + case PhysicsServer3D::BODY_STATE_SLEEPING: { + ERR_FAIL_V_MSG(false, "Sleeping state is not supported for Soft bodies."); + } break; + case PhysicsServer3D::BODY_STATE_CAN_SLEEP: { + ERR_FAIL_V_MSG(false, "Sleeping state is not supported for Soft bodies."); + } break; + } + + return Variant(); +} + +void SoftBody3DSW::set_space(Space3DSW *p_space) { + if (get_space()) { + get_space()->soft_body_remove_from_active_list(&active_list); + + deinitialize_shape(); + } + + _set_space(p_space); + + if (get_space()) { + get_space()->soft_body_add_to_active_list(&active_list); + + if (bounds != AABB()) { + initialize_shape(true); + } + } +} + +void SoftBody3DSW::set_mesh(const Ref<Mesh> &p_mesh) { + destroy(); + + soft_mesh = p_mesh; + + if (soft_mesh.is_null()) { + return; + } + + Array arrays = soft_mesh->surface_get_arrays(0); + ERR_FAIL_COND(!(soft_mesh->surface_get_format(0) & RS::ARRAY_FORMAT_INDEX)); + + bool success = create_from_trimesh(arrays[RS::ARRAY_INDEX], arrays[RS::ARRAY_VERTEX]); + if (!success) { + destroy(); + soft_mesh = Ref<Mesh>(); + } +} + +void SoftBody3DSW::update_rendering_server(RenderingServerHandler *p_rendering_server_handler) { + if (soft_mesh.is_null()) { + return; + } + + const uint32_t vertex_count = map_visual_to_physics.size(); + for (uint32_t i = 0; i < vertex_count; ++i) { + const uint32_t node_index = map_visual_to_physics[i]; + const Node &node = nodes[node_index]; + const Vector3 &vertex_position = node.x; + const Vector3 &vertex_normal = node.n; + + p_rendering_server_handler->set_vertex(i, &vertex_position); + p_rendering_server_handler->set_normal(i, &vertex_normal); + } + + p_rendering_server_handler->set_aabb(bounds); +} + +void SoftBody3DSW::update_normals() { + uint32_t i, ni; + + for (i = 0, ni = nodes.size(); i < ni; ++i) { + nodes[i].n = Vector3(); + } + + for (i = 0, ni = faces.size(); i < ni; ++i) { + Face &face = faces[i]; + const Vector3 n = vec3_cross(face.n[0]->x - face.n[2]->x, face.n[0]->x - face.n[1]->x); + face.n[0]->n += n; + face.n[1]->n += n; + face.n[2]->n += n; + face.normal = n; + face.normal.normalize(); + } + + for (i = 0, ni = nodes.size(); i < ni; ++i) { + Node &node = nodes[i]; + real_t len = node.n.length(); + if (len > CMP_EPSILON) { + node.n /= len; + } + } +} + +void SoftBody3DSW::update_bounds() { + AABB prev_bounds = bounds; + prev_bounds.grow_by(collision_margin); + + bounds = AABB(); + + const uint32_t nodes_count = nodes.size(); + if (nodes_count == 0) { + deinitialize_shape(); + return; + } + + bool first = true; + bool moved = false; + for (uint32_t node_index = 0; node_index < nodes_count; ++node_index) { + const Node &node = nodes[node_index]; + if (!prev_bounds.has_point(node.x)) { + moved = true; + } + if (first) { + bounds.position = node.x; + first = false; + } else { + bounds.expand_to(node.x); + } + } + + if (get_space()) { + initialize_shape(moved); + } +} + +void SoftBody3DSW::update_constants() { + reset_link_rest_lengths(); + update_link_constants(); + update_area(); +} + +void SoftBody3DSW::update_area() { + int i, ni; + + // Face area. + for (i = 0, ni = faces.size(); i < ni; ++i) { + Face &face = faces[i]; + + const Vector3 &x0 = face.n[0]->x; + const Vector3 &x1 = face.n[1]->x; + const Vector3 &x2 = face.n[2]->x; + + const Vector3 a = x1 - x0; + const Vector3 b = x2 - x0; + const Vector3 cr = vec3_cross(a, b); + face.ra = cr.length(); + } + + // Node area. + LocalVector<int> counts; + counts.resize(nodes.size()); + memset(counts.ptr(), 0, counts.size() * sizeof(int)); + + for (i = 0, ni = nodes.size(); i < ni; ++i) { + nodes[i].area = 0.0; + } + + for (i = 0, ni = faces.size(); i < ni; ++i) { + const Face &face = faces[i]; + for (int j = 0; j < 3; ++j) { + const int index = (int)(face.n[j] - &nodes[0]); + counts[index]++; + face.n[j]->area += Math::abs(face.ra); + } + } + + for (i = 0, ni = nodes.size(); i < ni; ++i) { + if (counts[i] > 0) { + nodes[i].area /= (real_t)counts[i]; + } else { + nodes[i].area = 0.0; + } + } +} + +void SoftBody3DSW::reset_link_rest_lengths() { + for (uint32_t i = 0, ni = links.size(); i < ni; ++i) { + Link &link = links[i]; + link.rl = (link.n[0]->x - link.n[1]->x).length(); + link.c1 = link.rl * link.rl; + } +} + +void SoftBody3DSW::update_link_constants() { + real_t inv_linear_stiffness = 1.0 / linear_stiffness; + for (uint32_t i = 0, ni = links.size(); i < ni; ++i) { + Link &link = links[i]; + link.c0 = (link.n[0]->im + link.n[1]->im) * inv_linear_stiffness; + } +} + +void SoftBody3DSW::apply_nodes_transform(const Transform &p_transform) { + if (soft_mesh.is_null()) { + return; + } + + uint32_t node_count = nodes.size(); + Vector3 leaf_size = Vector3(collision_margin, collision_margin, collision_margin) * 2.0; + for (uint32_t node_index = 0; node_index < node_count; ++node_index) { + Node &node = nodes[node_index]; + + node.x = p_transform.xform(node.x); + node.q = node.x; + node.v = Vector3(); + node.bv = Vector3(); + + AABB node_aabb(node.x, leaf_size); + node_tree.update(node.leaf, node_aabb); + } + + face_tree.clear(); + + update_normals(); + update_bounds(); + update_constants(); +} + +Vector3 SoftBody3DSW::get_vertex_position(int p_index) const { + if (soft_mesh.is_null()) { + return Vector3(); + } + + ERR_FAIL_INDEX_V(p_index, (int)map_visual_to_physics.size(), Vector3()); + uint32_t node_index = map_visual_to_physics[p_index]; + + ERR_FAIL_COND_V(node_index >= nodes.size(), Vector3()); + return nodes[node_index].x; +} + +void SoftBody3DSW::set_vertex_position(int p_index, const Vector3 &p_position) { + if (soft_mesh.is_null()) { + return; + } + + ERR_FAIL_INDEX(p_index, (int)map_visual_to_physics.size()); + uint32_t node_index = map_visual_to_physics[p_index]; + + ERR_FAIL_COND(node_index >= nodes.size()); + Node &node = nodes[node_index]; + node.q = node.x; + node.x = p_position; +} + +void SoftBody3DSW::pin_vertex(int p_index) { + if (is_vertex_pinned(p_index)) { + return; + } + + pinned_vertices.push_back(p_index); + + if (!soft_mesh.is_null()) { + ERR_FAIL_INDEX(p_index, (int)map_visual_to_physics.size()); + uint32_t node_index = map_visual_to_physics[p_index]; + + ERR_FAIL_COND(node_index >= nodes.size()); + Node &node = nodes[node_index]; + node.im = 0.0; + } +} + +void SoftBody3DSW::unpin_vertex(int p_index) { + uint32_t pinned_count = pinned_vertices.size(); + for (uint32_t i = 0; i < pinned_count; ++i) { + if (p_index == pinned_vertices[i]) { + pinned_vertices.remove(i); + + if (!soft_mesh.is_null()) { + ERR_FAIL_INDEX(p_index, (int)map_visual_to_physics.size()); + uint32_t node_index = map_visual_to_physics[p_index]; + + ERR_FAIL_COND(node_index >= nodes.size()); + real_t inv_node_mass = nodes.size() * inv_total_mass; + + Node &node = nodes[node_index]; + node.im = inv_node_mass; + } + + return; + } + } +} + +void SoftBody3DSW::unpin_all_vertices() { + if (!soft_mesh.is_null()) { + real_t inv_node_mass = nodes.size() * inv_total_mass; + uint32_t pinned_count = pinned_vertices.size(); + for (uint32_t i = 0; i < pinned_count; ++i) { + uint32_t vertex_index = pinned_vertices[i]; + + ERR_CONTINUE(vertex_index >= map_visual_to_physics.size()); + uint32_t node_index = map_visual_to_physics[vertex_index]; + + ERR_CONTINUE(node_index >= nodes.size()); + Node &node = nodes[node_index]; + node.im = inv_node_mass; + } + } + + pinned_vertices.clear(); +} + +bool SoftBody3DSW::is_vertex_pinned(int p_index) const { + uint32_t pinned_count = pinned_vertices.size(); + for (uint32_t i = 0; i < pinned_count; ++i) { + if (p_index == pinned_vertices[i]) { + return true; + } + } + + return false; +} + +uint32_t SoftBody3DSW::get_node_count() const { + return nodes.size(); +} + +real_t SoftBody3DSW::get_node_inv_mass(uint32_t p_node_index) const { + ERR_FAIL_COND_V(p_node_index >= nodes.size(), 0.0); + return nodes[p_node_index].im; +} + +Vector3 SoftBody3DSW::get_node_position(uint32_t p_node_index) const { + ERR_FAIL_COND_V(p_node_index >= nodes.size(), Vector3()); + return nodes[p_node_index].x; +} + +Vector3 SoftBody3DSW::get_node_velocity(uint32_t p_node_index) const { + ERR_FAIL_COND_V(p_node_index >= nodes.size(), Vector3()); + return nodes[p_node_index].v; +} + +Vector3 SoftBody3DSW::get_node_biased_velocity(uint32_t p_node_index) const { + ERR_FAIL_COND_V(p_node_index >= nodes.size(), Vector3()); + return nodes[p_node_index].bv; +} + +void SoftBody3DSW::apply_node_impulse(uint32_t p_node_index, const Vector3 &p_impulse) { + ERR_FAIL_COND(p_node_index >= nodes.size()); + Node &node = nodes[p_node_index]; + node.v += p_impulse * node.im; +} + +void SoftBody3DSW::apply_node_bias_impulse(uint32_t p_node_index, const Vector3 &p_impulse) { + ERR_FAIL_COND(p_node_index >= nodes.size()); + Node &node = nodes[p_node_index]; + node.bv += p_impulse * node.im; +} + +uint32_t SoftBody3DSW::get_face_count() const { + return faces.size(); +} + +void SoftBody3DSW::get_face_points(uint32_t p_face_index, Vector3 &r_point_1, Vector3 &r_point_2, Vector3 &r_point_3) const { + ERR_FAIL_COND(p_face_index >= faces.size()); + const Face &face = faces[p_face_index]; + r_point_1 = face.n[0]->x; + r_point_2 = face.n[1]->x; + r_point_3 = face.n[2]->x; +} + +Vector3 SoftBody3DSW::get_face_normal(uint32_t p_face_index) const { + ERR_FAIL_COND_V(p_face_index >= faces.size(), Vector3()); + return faces[p_face_index].normal; +} + +bool SoftBody3DSW::create_from_trimesh(const Vector<int> &p_indices, const Vector<Vector3> &p_vertices) { + uint32_t node_count = 0; + LocalVector<Vector3> vertices; + const int visual_vertex_count(p_vertices.size()); + + LocalVector<int> triangles; + const uint32_t triangle_count(p_indices.size() / 3); + triangles.resize(triangle_count * 3); + + // Merge all overlapping vertices and create a map of physical vertices to visual vertices. + { + // Process vertices. + { + uint32_t vertex_count = 0; + Map<Vector3, uint32_t> unique_vertices; + + vertices.resize(visual_vertex_count); + map_visual_to_physics.resize(visual_vertex_count); + + for (int visual_vertex_index = 0; visual_vertex_index < visual_vertex_count; ++visual_vertex_index) { + const Vector3 &vertex = p_vertices[visual_vertex_index]; + + Map<Vector3, uint32_t>::Element *e = unique_vertices.find(vertex); + uint32_t vertex_id; + if (e) { + // Already existing. + vertex_id = e->value(); + } else { + // Create new one. + vertex_id = vertex_count++; + unique_vertices[vertex] = vertex_id; + vertices[vertex_id] = vertex; + } + + map_visual_to_physics[visual_vertex_index] = vertex_id; + } + + vertices.resize(vertex_count); + } + + // Process triangles. + { + for (uint32_t triangle_index = 0; triangle_index < triangle_count; ++triangle_index) { + for (int i = 0; i < 3; ++i) { + int visual_index = 3 * triangle_index + i; + int physics_index = map_visual_to_physics[p_indices[visual_index]]; + triangles[visual_index] = physics_index; + node_count = MAX((int)node_count, physics_index); + } + } + } + } + + ++node_count; + + // Create nodes from vertices. + nodes.resize(node_count); + real_t inv_node_mass = node_count * inv_total_mass; + Vector3 leaf_size = Vector3(collision_margin, collision_margin, collision_margin) * 2.0; + for (uint32_t i = 0; i < node_count; ++i) { + Node &node = nodes[i]; + node.s = vertices[i]; + node.x = node.s; + node.q = node.s; + node.im = inv_node_mass; + + AABB node_aabb(node.x, leaf_size); + node.leaf = node_tree.insert(node_aabb, &node); + + node.index = i; + } + + // Create links and faces from triangles. + LocalVector<bool> chks; + chks.resize(node_count * node_count); + memset(chks.ptr(), 0, chks.size() * sizeof(bool)); + + for (uint32_t i = 0; i < triangle_count * 3; i += 3) { + const int idx[] = { triangles[i], triangles[i + 1], triangles[i + 2] }; + + for (int j = 2, k = 0; k < 3; j = k++) { + int chk = idx[k] * node_count + idx[j]; + if (!chks[chk]) { + chks[chk] = true; + int inv_chk = idx[j] * node_count + idx[k]; + chks[inv_chk] = true; + + append_link(idx[j], idx[k]); + } + } + + append_face(idx[0], idx[1], idx[2]); + } + + // Set pinned nodes. + uint32_t pinned_count = pinned_vertices.size(); + for (uint32_t i = 0; i < pinned_count; ++i) { + int pinned_vertex = pinned_vertices[i]; + + ERR_CONTINUE(pinned_vertex >= visual_vertex_count); + uint32_t node_index = map_visual_to_physics[pinned_vertex]; + + ERR_CONTINUE(node_index >= node_count); + Node &node = nodes[node_index]; + node.im = 0.0; + } + + generate_bending_constraints(2); + reoptimize_link_order(); + + update_constants(); + update_normals(); + update_bounds(); + + return true; +} + +void SoftBody3DSW::generate_bending_constraints(int p_distance) { + uint32_t i, j; + + if (p_distance > 1) { + // Build graph. + const uint32_t n = nodes.size(); + const unsigned inf = (~(unsigned)0) >> 1; + const uint32_t adj_size = n * n; + unsigned *adj = memnew_arr(unsigned, adj_size); + +#define IDX(_x_, _y_) ((_y_)*n + (_x_)) + for (j = 0; j < n; ++j) { + for (i = 0; i < n; ++i) { + int idx_ij = j * n + i; + int idx_ji = i * n + j; + if (i != j) { + adj[idx_ij] = adj[idx_ji] = inf; + } else { + adj[idx_ij] = adj[idx_ji] = 0; + } + } + } + for (i = 0; i < links.size(); ++i) { + const int ia = (int)(links[i].n[0] - &nodes[0]); + const int ib = (int)(links[i].n[1] - &nodes[0]); + int idx = ib * n + ia; + int idx_inv = ia * n + ib; + adj[idx] = 1; + adj[idx_inv] = 1; + } + + // Special optimized case for distance == 2. + if (p_distance == 2) { + LocalVector<LocalVector<int>> node_links; + + // Build node links. + node_links.resize(nodes.size()); + + for (i = 0; i < links.size(); ++i) { + const int ia = (int)(links[i].n[0] - &nodes[0]); + const int ib = (int)(links[i].n[1] - &nodes[0]); + if (node_links[ia].find(ib) == -1) { + node_links[ia].push_back(ib); + } + + if (node_links[ib].find(ia) == -1) { + node_links[ib].push_back(ia); + } + } + for (uint32_t ii = 0; ii < node_links.size(); ii++) { + for (uint32_t jj = 0; jj < node_links[ii].size(); jj++) { + int k = node_links[ii][jj]; + for (uint32_t kk = 0; kk < node_links[k].size(); kk++) { + int l = node_links[k][kk]; + if ((int)ii != l) { + int idx_ik = k * n + ii; + int idx_kj = l * n + k; + const unsigned sum = adj[idx_ik] + adj[idx_kj]; + ERR_FAIL_COND(sum != 2); + int idx_ij = l * n + ii; + if (adj[idx_ij] > sum) { + int idx_ji = l * n + ii; + adj[idx_ij] = adj[idx_ji] = sum; + } + } + } + } + } + } else { + // Generic Floyd's algorithm. + for (uint32_t k = 0; k < n; ++k) { + for (j = 0; j < n; ++j) { + for (i = j + 1; i < n; ++i) { + int idx_ik = k * n + i; + int idx_kj = j * n + k; + const unsigned sum = adj[idx_ik] + adj[idx_kj]; + int idx_ij = j * n + i; + if (adj[idx_ij] > sum) { + int idx_ji = j * n + i; + adj[idx_ij] = adj[idx_ji] = sum; + } + } + } + } + } + + // Build links. + for (j = 0; j < n; ++j) { + for (i = j + 1; i < n; ++i) { + int idx_ij = j * n + i; + if (adj[idx_ij] == (unsigned)p_distance) { + append_link(i, j); + } + } + } + memdelete_arr(adj); + } +} + +//=================================================================== +// +// +// This function takes in a list of interdependent Links and tries +// to maximize the distance between calculation +// of dependent links. This increases the amount of parallelism that can +// be exploited by out-of-order instruction processors with large but +// (inevitably) finite instruction windows. +// +//=================================================================== + +// A small structure to track lists of dependent link calculations. +class LinkDeps { +public: + int value; // A link calculation that is dependent on this one + // Positive values = "input A" while negative values = "input B" + LinkDeps *next; // Next dependence in the list +}; +typedef LinkDeps *LinkDepsPtr; + +void SoftBody3DSW::reoptimize_link_order() { + const int reop_not_dependent = -1; + const int reop_node_complete = -2; + + uint32_t i, link_count = links.size(), node_count = nodes.size(); + Link *lr; + int ar, br; + Node *node0 = &(nodes[0]); + Node *node1 = &(nodes[1]); + LinkDepsPtr link_dep; + int ready_list_head, ready_list_tail, link_num, link_dep_frees, dep_link; + + // Allocate temporary buffers. + int *node_written_at = memnew_arr(int, node_count + 1); // What link calculation produced this node's current values? + int *link_dep_A = memnew_arr(int, link_count); // Link calculation input is dependent upon prior calculation #N + int *link_dep_B = memnew_arr(int, link_count); + int *ready_list = memnew_arr(int, link_count); // List of ready-to-process link calculations (# of links, maximum) + LinkDeps *link_dep_free_list = memnew_arr(LinkDeps, 2 * link_count); // Dependent-on-me list elements (2x# of links, maximum) + LinkDepsPtr *link_dep_list_starts = memnew_arr(LinkDepsPtr, link_count); // Start nodes of dependent-on-me lists, one for each link + + // Copy the original, unsorted links to a side buffer. + Link *link_buffer = memnew_arr(Link, link_count); + memcpy(link_buffer, &(links[0]), sizeof(Link) * link_count); + + // Clear out the node setup and ready list. + for (i = 0; i < node_count + 1; i++) { + node_written_at[i] = reop_not_dependent; + } + for (i = 0; i < link_count; i++) { + link_dep_list_starts[i] = nullptr; + } + ready_list_head = ready_list_tail = link_dep_frees = 0; + + // Initial link analysis to set up data structures. + for (i = 0; i < link_count; i++) { + // Note which prior link calculations we are dependent upon & build up dependence lists. + lr = &(links[i]); + ar = (lr->n[0] - node0) / (node1 - node0); + br = (lr->n[1] - node0) / (node1 - node0); + if (node_written_at[ar] > reop_not_dependent) { + link_dep_A[i] = node_written_at[ar]; + link_dep = &link_dep_free_list[link_dep_frees++]; + link_dep->value = i; + link_dep->next = link_dep_list_starts[node_written_at[ar]]; + link_dep_list_starts[node_written_at[ar]] = link_dep; + } else { + link_dep_A[i] = reop_not_dependent; + } + if (node_written_at[br] > reop_not_dependent) { + link_dep_B[i] = node_written_at[br]; + link_dep = &link_dep_free_list[link_dep_frees++]; + link_dep->value = -(int)(i + 1); + link_dep->next = link_dep_list_starts[node_written_at[br]]; + link_dep_list_starts[node_written_at[br]] = link_dep; + } else { + link_dep_B[i] = reop_not_dependent; + } + + // Add this link to the initial ready list, if it is not dependent on any other links. + if ((link_dep_A[i] == reop_not_dependent) && (link_dep_B[i] == reop_not_dependent)) { + ready_list[ready_list_tail++] = i; + link_dep_A[i] = link_dep_B[i] = reop_node_complete; // Probably not needed now. + } + + // Update the nodes to mark which ones are calculated by this link. + node_written_at[ar] = node_written_at[br] = i; + } + + // Process the ready list and create the sorted list of links: + // -- By treating the ready list as a queue, we maximize the distance between any + // inter-dependent node calculations. + // -- All other (non-related) nodes in the ready list will automatically be inserted + // in between each set of inter-dependent link calculations by this loop. + i = 0; + while (ready_list_head != ready_list_tail) { + // Use ready list to select the next link to process. + link_num = ready_list[ready_list_head++]; + // Copy the next-to-calculate link back into the original link array. + links[i++] = link_buffer[link_num]; + + // Free up any link inputs that are dependent on this one. + link_dep = link_dep_list_starts[link_num]; + while (link_dep) { + dep_link = link_dep->value; + if (dep_link >= 0) { + link_dep_A[dep_link] = reop_not_dependent; + } else { + dep_link = -dep_link - 1; + link_dep_B[dep_link] = reop_not_dependent; + } + // Add this dependent link calculation to the ready list if *both* inputs are clear. + if ((link_dep_A[dep_link] == reop_not_dependent) && (link_dep_B[dep_link] == reop_not_dependent)) { + ready_list[ready_list_tail++] = dep_link; + link_dep_A[dep_link] = link_dep_B[dep_link] = reop_node_complete; // Probably not needed now. + } + link_dep = link_dep->next; + } + } + + // Delete the temporary buffers. + memdelete_arr(node_written_at); + memdelete_arr(link_dep_A); + memdelete_arr(link_dep_B); + memdelete_arr(ready_list); + memdelete_arr(link_dep_free_list); + memdelete_arr(link_dep_list_starts); + memdelete_arr(link_buffer); +} + +void SoftBody3DSW::append_link(uint32_t p_node1, uint32_t p_node2) { + if (p_node1 == p_node2) { + return; + } + + Node *node1 = &nodes[p_node1]; + Node *node2 = &nodes[p_node2]; + + Link link; + link.n[0] = node1; + link.n[1] = node2; + link.rl = (node1->x - node2->x).length(); + + links.push_back(link); +} + +void SoftBody3DSW::append_face(uint32_t p_node1, uint32_t p_node2, uint32_t p_node3) { + if (p_node1 == p_node2) { + return; + } + if (p_node1 == p_node3) { + return; + } + if (p_node2 == p_node3) { + return; + } + + Node *node1 = &nodes[p_node1]; + Node *node2 = &nodes[p_node2]; + Node *node3 = &nodes[p_node3]; + + Face face; + face.n[0] = node1; + face.n[1] = node2; + face.n[2] = node3; + + face.index = faces.size(); + + faces.push_back(face); +} + +void SoftBody3DSW::set_iteration_count(int p_val) { + iteration_count = p_val; +} + +void SoftBody3DSW::set_total_mass(real_t p_val) { + ERR_FAIL_COND(p_val < 0.0); + + inv_total_mass = 1.0 / p_val; + real_t mass_factor = total_mass * inv_total_mass; + total_mass = p_val; + + uint32_t node_count = nodes.size(); + for (uint32_t node_index = 0; node_index < node_count; ++node_index) { + Node &node = nodes[node_index]; + node.im *= mass_factor; + } + + update_constants(); +} + +void SoftBody3DSW::set_collision_margin(real_t p_val) { + collision_margin = p_val; +} + +void SoftBody3DSW::set_linear_stiffness(real_t p_val) { + linear_stiffness = p_val; +} + +void SoftBody3DSW::set_pressure_coefficient(real_t p_val) { + pressure_coefficient = p_val; +} + +void SoftBody3DSW::set_damping_coefficient(real_t p_val) { + damping_coefficient = p_val; +} + +void SoftBody3DSW::set_drag_coefficient(real_t p_val) { + drag_coefficient = p_val; +} + +void SoftBody3DSW::add_velocity(const Vector3 &p_velocity) { + for (uint32_t i = 0, ni = nodes.size(); i < ni; ++i) { + Node &node = nodes[i]; + if (node.im > 0) { + node.v += p_velocity; + } + } +} + +void SoftBody3DSW::apply_forces() { + if (pressure_coefficient < CMP_EPSILON) { + return; + } + + if (nodes.is_empty()) { + return; + } + + uint32_t i, ni; + + // Calculate volume. + real_t volume = 0.0; + const Vector3 &org = nodes[0].x; + for (i = 0, ni = faces.size(); i < ni; ++i) { + const Face &face = faces[i]; + volume += vec3_dot(face.n[0]->x - org, vec3_cross(face.n[1]->x - org, face.n[2]->x - org)); + } + volume /= 6.0; + + // Apply per node forces. + real_t ivolumetp = 1.0 / Math::abs(volume) * pressure_coefficient; + for (i = 0, ni = nodes.size(); i < ni; ++i) { + Node &node = nodes[i]; + if (node.im > 0) { + node.f += node.n * (node.area * ivolumetp); + } + } +} + +void SoftBody3DSW::predict_motion(real_t p_delta) { + const real_t inv_delta = 1.0 / p_delta; + + ERR_FAIL_COND(!get_space()); + + Area3DSW *def_area = get_space()->get_default_area(); + ERR_FAIL_COND(!def_area); + + // Apply forces. + Vector3 gravity = def_area->get_gravity_vector() * def_area->get_gravity(); + add_velocity(gravity * p_delta); + apply_forces(); + + // Avoid soft body from 'exploding' so use some upper threshold of maximum motion + // that a node can travel per frame. + const real_t max_displacement = 1000.0; + real_t clamp_delta_v = max_displacement * inv_delta; + + // Integrate. + uint32_t i, ni; + for (i = 0, ni = nodes.size(); i < ni; ++i) { + Node &node = nodes[i]; + node.q = node.x; + Vector3 delta_v = node.f * node.im * p_delta; + for (int c = 0; c < 3; c++) { + delta_v[c] = CLAMP(delta_v[c], -clamp_delta_v, clamp_delta_v); + } + node.v += delta_v; + node.x += node.v * p_delta; + node.f = Vector3(); + } + + // Bounds and tree update. + update_bounds(); + + // Node tree update. + for (i = 0, ni = nodes.size(); i < ni; ++i) { + const Node &node = nodes[i]; + + AABB node_aabb(node.x, Vector3()); + node_aabb.expand_to(node.x + node.v * p_delta); + node_aabb.grow_by(collision_margin); + + node_tree.update(node.leaf, node_aabb); + } + + // Face tree update. + if (!face_tree.is_empty()) { + update_face_tree(p_delta); + } + + // Optimize node tree. + node_tree.optimize_incremental(1); + face_tree.optimize_incremental(1); +} + +void SoftBody3DSW::solve_constraints(real_t p_delta) { + const real_t inv_delta = 1.0 / p_delta; + + uint32_t i, ni; + + for (i = 0, ni = links.size(); i < ni; ++i) { + Link &link = links[i]; + link.c3 = link.n[1]->q - link.n[0]->q; + link.c2 = 1 / (link.c3.length_squared() * link.c0); + } + + // Solve velocities. + for (i = 0, ni = nodes.size(); i < ni; ++i) { + Node &node = nodes[i]; + node.x = node.q + node.v * p_delta; + } + + // Solve positions. + for (int isolve = 0; isolve < iteration_count; ++isolve) { + const real_t ti = isolve / (real_t)iteration_count; + solve_links(1.0, ti); + } + const real_t vc = (1.0 - damping_coefficient) * inv_delta; + for (i = 0, ni = nodes.size(); i < ni; ++i) { + Node &node = nodes[i]; + + node.x += node.bv * p_delta; + node.bv = Vector3(); + + node.v = (node.x - node.q) * vc; + + node.q = node.x; + } + + update_normals(); +} + +void SoftBody3DSW::solve_links(real_t kst, real_t ti) { + for (uint32_t i = 0, ni = links.size(); i < ni; ++i) { + Link &link = links[i]; + if (link.c0 > 0) { + Node &node_a = *link.n[0]; + Node &node_b = *link.n[1]; + const Vector3 del = node_b.x - node_a.x; + const real_t len = del.length_squared(); + if (link.c1 + len > CMP_EPSILON) { + const real_t k = ((link.c1 - len) / (link.c0 * (link.c1 + len))) * kst; + node_a.x -= del * (k * node_a.im); + node_b.x += del * (k * node_b.im); + } + } + } +} + +struct AABBQueryResult { + const SoftBody3DSW *soft_body = nullptr; + void *userdata = nullptr; + SoftBody3DSW::QueryResultCallback result_callback = nullptr; + + _FORCE_INLINE_ bool operator()(void *p_data) { + return result_callback(soft_body->get_node_index(p_data), userdata); + }; +}; + +void SoftBody3DSW::query_aabb(const AABB &p_aabb, SoftBody3DSW::QueryResultCallback p_result_callback, void *p_userdata) { + AABBQueryResult query_result; + query_result.soft_body = this; + query_result.result_callback = p_result_callback; + query_result.userdata = p_userdata; + + node_tree.aabb_query(p_aabb, query_result); +} + +struct RayQueryResult { + const SoftBody3DSW *soft_body = nullptr; + void *userdata = nullptr; + SoftBody3DSW::QueryResultCallback result_callback = nullptr; + + _FORCE_INLINE_ bool operator()(void *p_data) { + return result_callback(soft_body->get_face_index(p_data), userdata); + }; +}; + +void SoftBody3DSW::query_ray(const Vector3 &p_from, const Vector3 &p_to, SoftBody3DSW::QueryResultCallback p_result_callback, void *p_userdata) { + if (face_tree.is_empty()) { + initialize_face_tree(); + } + + RayQueryResult query_result; + query_result.soft_body = this; + query_result.result_callback = p_result_callback; + query_result.userdata = p_userdata; + + face_tree.ray_query(p_from, p_to, query_result); +} + +void SoftBody3DSW::initialize_face_tree() { + face_tree.clear(); + for (uint32_t i = 0; i < faces.size(); ++i) { + Face &face = faces[i]; + + AABB face_aabb; + + face_aabb.position = face.n[0]->x; + face_aabb.expand_to(face.n[1]->x); + face_aabb.expand_to(face.n[2]->x); + + face_aabb.grow_by(collision_margin); + + face.leaf = face_tree.insert(face_aabb, &face); + } +} + +void SoftBody3DSW::update_face_tree(real_t p_delta) { + for (uint32_t i = 0; i < faces.size(); ++i) { + const Face &face = faces[i]; + + AABB face_aabb; + + const Node *node0 = face.n[0]; + face_aabb.position = node0->x; + face_aabb.expand_to(node0->x + node0->v * p_delta); + + const Node *node1 = face.n[1]; + face_aabb.expand_to(node1->x); + face_aabb.expand_to(node1->x + node1->v * p_delta); + + const Node *node2 = face.n[2]; + face_aabb.expand_to(node2->x); + face_aabb.expand_to(node2->x + node2->v * p_delta); + + face_aabb.grow_by(collision_margin); + + face_tree.update(face.leaf, face_aabb); + } +} + +void SoftBody3DSW::initialize_shape(bool p_force_move) { + if (get_shape_count() == 0) { + SoftBodyShape3DSW *soft_body_shape = memnew(SoftBodyShape3DSW(this)); + add_shape(soft_body_shape); + } else if (p_force_move) { + SoftBodyShape3DSW *soft_body_shape = static_cast<SoftBodyShape3DSW *>(get_shape(0)); + soft_body_shape->update_bounds(); + } +} + +void SoftBody3DSW::deinitialize_shape() { + if (get_shape_count() > 0) { + Shape3DSW *shape = get_shape(0); + remove_shape(shape); + memdelete(shape); + } +} + +void SoftBody3DSW::destroy() { + map_visual_to_physics.clear(); + + node_tree.clear(); + face_tree.clear(); + + nodes.clear(); + links.clear(); + faces.clear(); + + bounds = AABB(); + deinitialize_shape(); +} + +void SoftBodyShape3DSW::update_bounds() { + ERR_FAIL_COND(!soft_body); + + AABB collision_aabb = soft_body->get_bounds(); + collision_aabb.grow_by(soft_body->get_collision_margin()); + configure(collision_aabb); +} + +SoftBodyShape3DSW::SoftBodyShape3DSW(SoftBody3DSW *p_soft_body) { + soft_body = p_soft_body; + update_bounds(); +} + +struct _SoftBodyIntersectSegmentInfo { + const SoftBody3DSW *soft_body = nullptr; + Vector3 from; + Vector3 dir; + Vector3 hit_position; + uint32_t hit_face_index = -1; + real_t hit_dist_sq = Math_INF; + + static bool process_hit(uint32_t p_face_index, void *p_userdata) { + _SoftBodyIntersectSegmentInfo &query_info = *(_SoftBodyIntersectSegmentInfo *)(p_userdata); + + Vector3 points[3]; + query_info.soft_body->get_face_points(p_face_index, points[0], points[1], points[2]); + + Vector3 result; + if (Geometry3D::ray_intersects_triangle(query_info.from, query_info.dir, points[0], points[1], points[2], &result)) { + real_t dist_sq = query_info.from.distance_squared_to(result); + if (dist_sq < query_info.hit_dist_sq) { + query_info.hit_dist_sq = dist_sq; + query_info.hit_position = result; + query_info.hit_face_index = p_face_index; + } + } + + // Continue with the query. + return false; + } +}; + +bool SoftBodyShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { + _SoftBodyIntersectSegmentInfo query_info; + query_info.soft_body = soft_body; + query_info.from = p_begin; + query_info.dir = (p_end - p_begin).normalized(); + + soft_body->query_ray(p_begin, p_end, _SoftBodyIntersectSegmentInfo::process_hit, &query_info); + + if (query_info.hit_dist_sq != Math_INF) { + r_result = query_info.hit_position; + r_normal = soft_body->get_face_normal(query_info.hit_face_index); + return true; + } + + return false; +} + +bool SoftBodyShape3DSW::intersect_point(const Vector3 &p_point) const { + return false; +} + +Vector3 SoftBodyShape3DSW::get_closest_point_to(const Vector3 &p_point) const { + ERR_FAIL_V_MSG(Vector3(), "Get closest point is not supported for soft bodies."); +} |