1/**************************************************************************/
2/* godot_soft_body_3d.cpp */
3/**************************************************************************/
4/* This file is part of: */
5/* GODOT ENGINE */
6/* https://godotengine.org */
7/**************************************************************************/
8/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
9/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
10/* */
11/* Permission is hereby granted, free of charge, to any person obtaining */
12/* a copy of this software and associated documentation files (the */
13/* "Software"), to deal in the Software without restriction, including */
14/* without limitation the rights to use, copy, modify, merge, publish, */
15/* distribute, sublicense, and/or sell copies of the Software, and to */
16/* permit persons to whom the Software is furnished to do so, subject to */
17/* the following conditions: */
18/* */
19/* The above copyright notice and this permission notice shall be */
20/* included in all copies or substantial portions of the Software. */
21/* */
22/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
23/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
24/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
25/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
26/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
27/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
28/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
29/**************************************************************************/
30
31#include "godot_soft_body_3d.h"
32
33#include "godot_space_3d.h"
34
35#include "core/math/geometry_3d.h"
36#include "core/templates/rb_map.h"
37#include "servers/rendering_server.h"
38
39// Based on Bullet soft body.
40
41/*
42Bullet Continuous Collision Detection and Physics Library
43Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
44
45This software is provided 'as-is', without any express or implied warranty.
46In no event will the authors be held liable for any damages arising from the use of this software.
47Permission is granted to anyone to use this software for any purpose,
48including commercial applications, and to alter it and redistribute it freely,
49subject to the following restrictions:
50
511. 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.
522. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
533. This notice may not be removed or altered from any source distribution.
54*/
55///btSoftBody implementation by Nathanael Presson
56
57GodotSoftBody3D::GodotSoftBody3D() :
58 GodotCollisionObject3D(TYPE_SOFT_BODY),
59 active_list(this) {
60 _set_static(false);
61}
62
63void GodotSoftBody3D::_shapes_changed() {
64}
65
66void GodotSoftBody3D::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_variant) {
67 switch (p_state) {
68 case PhysicsServer3D::BODY_STATE_TRANSFORM: {
69 _set_transform(p_variant);
70 _set_inv_transform(get_transform().inverse());
71
72 apply_nodes_transform(get_transform());
73
74 } break;
75 case PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY: {
76 // Not supported.
77 ERR_FAIL_MSG("Linear velocity is not supported for Soft bodies.");
78 } break;
79 case PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY: {
80 ERR_FAIL_MSG("Angular velocity is not supported for Soft bodies.");
81 } break;
82 case PhysicsServer3D::BODY_STATE_SLEEPING: {
83 ERR_FAIL_MSG("Sleeping state is not supported for Soft bodies.");
84 } break;
85 case PhysicsServer3D::BODY_STATE_CAN_SLEEP: {
86 ERR_FAIL_MSG("Sleeping state is not supported for Soft bodies.");
87 } break;
88 }
89}
90
91Variant GodotSoftBody3D::get_state(PhysicsServer3D::BodyState p_state) const {
92 switch (p_state) {
93 case PhysicsServer3D::BODY_STATE_TRANSFORM: {
94 return get_transform();
95 } break;
96 case PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY: {
97 ERR_FAIL_V_MSG(Vector3(), "Linear velocity is not supported for Soft bodies.");
98 } break;
99 case PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY: {
100 ERR_FAIL_V_MSG(Vector3(), "Angular velocity is not supported for Soft bodies.");
101 } break;
102 case PhysicsServer3D::BODY_STATE_SLEEPING: {
103 ERR_FAIL_V_MSG(false, "Sleeping state is not supported for Soft bodies.");
104 } break;
105 case PhysicsServer3D::BODY_STATE_CAN_SLEEP: {
106 ERR_FAIL_V_MSG(false, "Sleeping state is not supported for Soft bodies.");
107 } break;
108 }
109
110 return Variant();
111}
112
113void GodotSoftBody3D::set_space(GodotSpace3D *p_space) {
114 if (get_space()) {
115 get_space()->soft_body_remove_from_active_list(&active_list);
116
117 deinitialize_shape();
118 }
119
120 _set_space(p_space);
121
122 if (get_space()) {
123 get_space()->soft_body_add_to_active_list(&active_list);
124
125 if (bounds != AABB()) {
126 initialize_shape(true);
127 }
128 }
129}
130
131void GodotSoftBody3D::set_mesh(RID p_mesh) {
132 destroy();
133
134 soft_mesh = p_mesh;
135
136 if (soft_mesh.is_null()) {
137 return;
138 }
139
140 Array arrays = RenderingServer::get_singleton()->mesh_surface_get_arrays(soft_mesh, 0);
141 ERR_FAIL_COND(arrays.is_empty());
142
143 bool success = create_from_trimesh(arrays[RenderingServer::ARRAY_INDEX], arrays[RenderingServer::ARRAY_VERTEX]);
144 if (!success) {
145 destroy();
146 }
147}
148
149void GodotSoftBody3D::update_rendering_server(PhysicsServer3DRenderingServerHandler *p_rendering_server_handler) {
150 if (soft_mesh.is_null()) {
151 return;
152 }
153
154 const uint32_t vertex_count = map_visual_to_physics.size();
155 for (uint32_t i = 0; i < vertex_count; ++i) {
156 const uint32_t node_index = map_visual_to_physics[i];
157 const Node &node = nodes[node_index];
158 const Vector3 &vertex_position = node.x;
159 const Vector3 &vertex_normal = node.n;
160
161 p_rendering_server_handler->set_vertex(i, &vertex_position);
162 p_rendering_server_handler->set_normal(i, &vertex_normal);
163 }
164
165 p_rendering_server_handler->set_aabb(bounds);
166}
167
168void GodotSoftBody3D::update_normals_and_centroids() {
169 for (Node &node : nodes) {
170 node.n = Vector3();
171 }
172
173 for (Face &face : faces) {
174 const Vector3 n = vec3_cross(face.n[0]->x - face.n[2]->x, face.n[0]->x - face.n[1]->x);
175 face.n[0]->n += n;
176 face.n[1]->n += n;
177 face.n[2]->n += n;
178 face.normal = n;
179 face.normal.normalize();
180 face.centroid = 0.33333333333 * (face.n[0]->x + face.n[1]->x + face.n[2]->x);
181 }
182
183 for (Node &node : nodes) {
184 real_t len = node.n.length();
185 if (len > CMP_EPSILON) {
186 node.n /= len;
187 }
188 }
189}
190
191void GodotSoftBody3D::update_bounds() {
192 AABB prev_bounds = bounds;
193 prev_bounds.grow_by(collision_margin);
194
195 bounds = AABB();
196
197 const uint32_t nodes_count = nodes.size();
198 if (nodes_count == 0) {
199 deinitialize_shape();
200 return;
201 }
202
203 bool first = true;
204 bool moved = false;
205 for (uint32_t node_index = 0; node_index < nodes_count; ++node_index) {
206 const Node &node = nodes[node_index];
207 if (!prev_bounds.has_point(node.x)) {
208 moved = true;
209 }
210 if (first) {
211 bounds.position = node.x;
212 first = false;
213 } else {
214 bounds.expand_to(node.x);
215 }
216 }
217
218 if (get_space()) {
219 initialize_shape(moved);
220 }
221}
222
223void GodotSoftBody3D::update_constants() {
224 reset_link_rest_lengths();
225 update_link_constants();
226 update_area();
227}
228
229void GodotSoftBody3D::update_area() {
230 int i, ni;
231
232 // Face area.
233 for (Face &face : faces) {
234 const Vector3 &x0 = face.n[0]->x;
235 const Vector3 &x1 = face.n[1]->x;
236 const Vector3 &x2 = face.n[2]->x;
237
238 const Vector3 a = x1 - x0;
239 const Vector3 b = x2 - x0;
240 const Vector3 cr = vec3_cross(a, b);
241 face.ra = cr.length() * 0.5;
242 }
243
244 // Node area.
245 LocalVector<int> counts;
246 if (nodes.size() > 0) {
247 counts.resize(nodes.size());
248 memset(counts.ptr(), 0, counts.size() * sizeof(int));
249 }
250
251 for (Node &node : nodes) {
252 node.area = 0.0;
253 }
254
255 for (const Face &face : faces) {
256 for (int j = 0; j < 3; ++j) {
257 const int index = (int)(face.n[j] - &nodes[0]);
258 counts[index]++;
259 face.n[j]->area += Math::abs(face.ra);
260 }
261 }
262
263 for (i = 0, ni = nodes.size(); i < ni; ++i) {
264 if (counts[i] > 0) {
265 nodes[i].area /= (real_t)counts[i];
266 } else {
267 nodes[i].area = 0.0;
268 }
269 }
270}
271
272void GodotSoftBody3D::reset_link_rest_lengths() {
273 for (Link &link : links) {
274 link.rl = (link.n[0]->x - link.n[1]->x).length();
275 link.c1 = link.rl * link.rl;
276 }
277}
278
279void GodotSoftBody3D::update_link_constants() {
280 real_t inv_linear_stiffness = 1.0 / linear_stiffness;
281 for (Link &link : links) {
282 link.c0 = (link.n[0]->im + link.n[1]->im) * inv_linear_stiffness;
283 }
284}
285
286void GodotSoftBody3D::apply_nodes_transform(const Transform3D &p_transform) {
287 if (soft_mesh.is_null()) {
288 return;
289 }
290
291 uint32_t node_count = nodes.size();
292 Vector3 leaf_size = Vector3(collision_margin, collision_margin, collision_margin) * 2.0;
293 for (uint32_t node_index = 0; node_index < node_count; ++node_index) {
294 Node &node = nodes[node_index];
295
296 node.x = p_transform.xform(node.x);
297 node.q = node.x;
298 node.v = Vector3();
299 node.bv = Vector3();
300
301 AABB node_aabb(node.x, leaf_size);
302 node_tree.update(node.leaf, node_aabb);
303 }
304
305 face_tree.clear();
306
307 update_normals_and_centroids();
308 update_bounds();
309 update_constants();
310}
311
312Vector3 GodotSoftBody3D::get_vertex_position(int p_index) const {
313 ERR_FAIL_COND_V(p_index < 0, Vector3());
314
315 if (soft_mesh.is_null()) {
316 return Vector3();
317 }
318
319 ERR_FAIL_COND_V(p_index >= (int)map_visual_to_physics.size(), Vector3());
320 uint32_t node_index = map_visual_to_physics[p_index];
321
322 ERR_FAIL_COND_V(node_index >= nodes.size(), Vector3());
323 return nodes[node_index].x;
324}
325
326void GodotSoftBody3D::set_vertex_position(int p_index, const Vector3 &p_position) {
327 ERR_FAIL_COND(p_index < 0);
328
329 if (soft_mesh.is_null()) {
330 return;
331 }
332
333 ERR_FAIL_COND(p_index >= (int)map_visual_to_physics.size());
334 uint32_t node_index = map_visual_to_physics[p_index];
335
336 ERR_FAIL_COND(node_index >= nodes.size());
337 Node &node = nodes[node_index];
338 node.q = node.x;
339 node.x = p_position;
340}
341
342void GodotSoftBody3D::pin_vertex(int p_index) {
343 ERR_FAIL_COND(p_index < 0);
344
345 if (is_vertex_pinned(p_index)) {
346 return;
347 }
348
349 pinned_vertices.push_back(p_index);
350
351 if (!soft_mesh.is_null()) {
352 ERR_FAIL_COND(p_index >= (int)map_visual_to_physics.size());
353 uint32_t node_index = map_visual_to_physics[p_index];
354
355 ERR_FAIL_COND(node_index >= nodes.size());
356 Node &node = nodes[node_index];
357 node.im = 0.0;
358 }
359}
360
361void GodotSoftBody3D::unpin_vertex(int p_index) {
362 ERR_FAIL_COND(p_index < 0);
363
364 uint32_t pinned_count = pinned_vertices.size();
365 for (uint32_t i = 0; i < pinned_count; ++i) {
366 if (p_index == pinned_vertices[i]) {
367 pinned_vertices.remove_at(i);
368
369 if (!soft_mesh.is_null()) {
370 ERR_FAIL_COND(p_index >= (int)map_visual_to_physics.size());
371 uint32_t node_index = map_visual_to_physics[p_index];
372
373 ERR_FAIL_COND(node_index >= nodes.size());
374 real_t inv_node_mass = nodes.size() * inv_total_mass;
375
376 Node &node = nodes[node_index];
377 node.im = inv_node_mass;
378 }
379
380 return;
381 }
382 }
383}
384
385void GodotSoftBody3D::unpin_all_vertices() {
386 if (!soft_mesh.is_null()) {
387 real_t inv_node_mass = nodes.size() * inv_total_mass;
388 uint32_t pinned_count = pinned_vertices.size();
389 for (uint32_t i = 0; i < pinned_count; ++i) {
390 int pinned_vertex = pinned_vertices[i];
391
392 ERR_CONTINUE(pinned_vertex >= (int)map_visual_to_physics.size());
393 uint32_t node_index = map_visual_to_physics[pinned_vertex];
394
395 ERR_CONTINUE(node_index >= nodes.size());
396 Node &node = nodes[node_index];
397 node.im = inv_node_mass;
398 }
399 }
400
401 pinned_vertices.clear();
402}
403
404bool GodotSoftBody3D::is_vertex_pinned(int p_index) const {
405 ERR_FAIL_COND_V(p_index < 0, false);
406
407 uint32_t pinned_count = pinned_vertices.size();
408 for (uint32_t i = 0; i < pinned_count; ++i) {
409 if (p_index == pinned_vertices[i]) {
410 return true;
411 }
412 }
413
414 return false;
415}
416
417uint32_t GodotSoftBody3D::get_node_count() const {
418 return nodes.size();
419}
420
421real_t GodotSoftBody3D::get_node_inv_mass(uint32_t p_node_index) const {
422 ERR_FAIL_UNSIGNED_INDEX_V(p_node_index, nodes.size(), 0.0);
423 return nodes[p_node_index].im;
424}
425
426Vector3 GodotSoftBody3D::get_node_position(uint32_t p_node_index) const {
427 ERR_FAIL_UNSIGNED_INDEX_V(p_node_index, nodes.size(), Vector3());
428 return nodes[p_node_index].x;
429}
430
431Vector3 GodotSoftBody3D::get_node_velocity(uint32_t p_node_index) const {
432 ERR_FAIL_UNSIGNED_INDEX_V(p_node_index, nodes.size(), Vector3());
433 return nodes[p_node_index].v;
434}
435
436Vector3 GodotSoftBody3D::get_node_biased_velocity(uint32_t p_node_index) const {
437 ERR_FAIL_UNSIGNED_INDEX_V(p_node_index, nodes.size(), Vector3());
438 return nodes[p_node_index].bv;
439}
440
441void GodotSoftBody3D::apply_node_impulse(uint32_t p_node_index, const Vector3 &p_impulse) {
442 ERR_FAIL_UNSIGNED_INDEX(p_node_index, nodes.size());
443 Node &node = nodes[p_node_index];
444 node.v += p_impulse * node.im;
445}
446
447void GodotSoftBody3D::apply_node_bias_impulse(uint32_t p_node_index, const Vector3 &p_impulse) {
448 ERR_FAIL_UNSIGNED_INDEX(p_node_index, nodes.size());
449 Node &node = nodes[p_node_index];
450 node.bv += p_impulse * node.im;
451}
452
453uint32_t GodotSoftBody3D::get_face_count() const {
454 return faces.size();
455}
456
457void GodotSoftBody3D::get_face_points(uint32_t p_face_index, Vector3 &r_point_1, Vector3 &r_point_2, Vector3 &r_point_3) const {
458 ERR_FAIL_UNSIGNED_INDEX(p_face_index, faces.size());
459 const Face &face = faces[p_face_index];
460 r_point_1 = face.n[0]->x;
461 r_point_2 = face.n[1]->x;
462 r_point_3 = face.n[2]->x;
463}
464
465Vector3 GodotSoftBody3D::get_face_normal(uint32_t p_face_index) const {
466 ERR_FAIL_UNSIGNED_INDEX_V(p_face_index, faces.size(), Vector3());
467 return faces[p_face_index].normal;
468}
469
470bool GodotSoftBody3D::create_from_trimesh(const Vector<int> &p_indices, const Vector<Vector3> &p_vertices) {
471 ERR_FAIL_COND_V(p_indices.is_empty(), false);
472 ERR_FAIL_COND_V(p_vertices.is_empty(), false);
473
474 uint32_t node_count = 0;
475 LocalVector<Vector3> vertices;
476 const int visual_vertex_count(p_vertices.size());
477
478 LocalVector<int> triangles;
479 const uint32_t triangle_count(p_indices.size() / 3);
480 triangles.resize(triangle_count * 3);
481
482 // Merge all overlapping vertices and create a map of physical vertices to visual vertices.
483 {
484 // Process vertices.
485 {
486 uint32_t vertex_count = 0;
487 HashMap<Vector3, uint32_t> unique_vertices;
488
489 vertices.resize(visual_vertex_count);
490 map_visual_to_physics.resize(visual_vertex_count);
491
492 for (int visual_vertex_index = 0; visual_vertex_index < visual_vertex_count; ++visual_vertex_index) {
493 const Vector3 &vertex = p_vertices[visual_vertex_index];
494
495 HashMap<Vector3, uint32_t>::Iterator e = unique_vertices.find(vertex);
496 uint32_t vertex_id;
497 if (e) {
498 // Already existing.
499 vertex_id = e->value;
500 } else {
501 // Create new one.
502 vertex_id = vertex_count++;
503 unique_vertices[vertex] = vertex_id;
504 vertices[vertex_id] = vertex;
505 }
506
507 map_visual_to_physics[visual_vertex_index] = vertex_id;
508 }
509
510 vertices.resize(vertex_count);
511 }
512
513 // Process triangles.
514 {
515 for (uint32_t triangle_index = 0; triangle_index < triangle_count; ++triangle_index) {
516 for (int i = 0; i < 3; ++i) {
517 int visual_index = 3 * triangle_index + i;
518 int physics_index = map_visual_to_physics[p_indices[visual_index]];
519 triangles[visual_index] = physics_index;
520 node_count = MAX((int)node_count, physics_index);
521 }
522 }
523 }
524 }
525
526 ++node_count;
527
528 // Create nodes from vertices.
529 nodes.resize(node_count);
530 real_t inv_node_mass = node_count * inv_total_mass;
531 Vector3 leaf_size = Vector3(collision_margin, collision_margin, collision_margin) * 2.0;
532 for (uint32_t i = 0; i < node_count; ++i) {
533 Node &node = nodes[i];
534 node.s = vertices[i];
535 node.x = node.s;
536 node.q = node.s;
537 node.im = inv_node_mass;
538
539 AABB node_aabb(node.x, leaf_size);
540 node.leaf = node_tree.insert(node_aabb, &node);
541
542 node.index = i;
543 }
544
545 // Create links and faces from triangles.
546 LocalVector<bool> chks;
547 chks.resize(node_count * node_count);
548 memset(chks.ptr(), 0, chks.size() * sizeof(bool));
549
550 for (uint32_t i = 0; i < triangle_count * 3; i += 3) {
551 const int idx[] = { triangles[i], triangles[i + 1], triangles[i + 2] };
552
553 for (int j = 2, k = 0; k < 3; j = k++) {
554 int chk = idx[k] * node_count + idx[j];
555 if (!chks[chk]) {
556 chks[chk] = true;
557 int inv_chk = idx[j] * node_count + idx[k];
558 chks[inv_chk] = true;
559
560 append_link(idx[j], idx[k]);
561 }
562 }
563
564 append_face(idx[0], idx[1], idx[2]);
565 }
566
567 // Set pinned nodes.
568 uint32_t pinned_count = pinned_vertices.size();
569 for (uint32_t i = 0; i < pinned_count; ++i) {
570 int pinned_vertex = pinned_vertices[i];
571
572 ERR_CONTINUE(pinned_vertex >= visual_vertex_count);
573 uint32_t node_index = map_visual_to_physics[pinned_vertex];
574
575 ERR_CONTINUE(node_index >= node_count);
576 Node &node = nodes[node_index];
577 node.im = 0.0;
578 }
579
580 generate_bending_constraints(2);
581 reoptimize_link_order();
582
583 update_constants();
584 update_normals_and_centroids();
585 update_bounds();
586
587 return true;
588}
589
590void GodotSoftBody3D::generate_bending_constraints(int p_distance) {
591 uint32_t i, j;
592
593 if (p_distance > 1) {
594 // Build graph.
595 const uint32_t n = nodes.size();
596 const unsigned inf = (~(unsigned)0) >> 1;
597 const uint32_t adj_size = n * n;
598 unsigned *adj = memnew_arr(unsigned, adj_size);
599
600#define IDX(_x_, _y_) ((_y_)*n + (_x_))
601 for (j = 0; j < n; ++j) {
602 for (i = 0; i < n; ++i) {
603 int idx_ij = j * n + i;
604 int idx_ji = i * n + j;
605 if (i != j) {
606 adj[idx_ij] = adj[idx_ji] = inf;
607 } else {
608 adj[idx_ij] = adj[idx_ji] = 0;
609 }
610 }
611 }
612 for (Link &link : links) {
613 const int ia = (int)(link.n[0] - &nodes[0]);
614 const int ib = (int)(link.n[1] - &nodes[0]);
615 int idx = ib * n + ia;
616 int idx_inv = ia * n + ib;
617 adj[idx] = 1;
618 adj[idx_inv] = 1;
619 }
620
621 // Special optimized case for distance == 2.
622 if (p_distance == 2) {
623 LocalVector<LocalVector<int>> node_links;
624
625 // Build node links.
626 node_links.resize(nodes.size());
627
628 for (Link &link : links) {
629 const int ia = (int)(link.n[0] - &nodes[0]);
630 const int ib = (int)(link.n[1] - &nodes[0]);
631 if (node_links[ia].find(ib) == -1) {
632 node_links[ia].push_back(ib);
633 }
634
635 if (node_links[ib].find(ia) == -1) {
636 node_links[ib].push_back(ia);
637 }
638 }
639 for (uint32_t ii = 0; ii < node_links.size(); ii++) {
640 for (uint32_t jj = 0; jj < node_links[ii].size(); jj++) {
641 int k = node_links[ii][jj];
642 for (const int &l : node_links[k]) {
643 if ((int)ii != l) {
644 int idx_ik = k * n + ii;
645 int idx_kj = l * n + k;
646 const unsigned sum = adj[idx_ik] + adj[idx_kj];
647 ERR_FAIL_COND(sum != 2);
648 int idx_ij = l * n + ii;
649 if (adj[idx_ij] > sum) {
650 int idx_ji = l * n + ii;
651 adj[idx_ij] = adj[idx_ji] = sum;
652 }
653 }
654 }
655 }
656 }
657 } else {
658 // Generic Floyd's algorithm.
659 for (uint32_t k = 0; k < n; ++k) {
660 for (j = 0; j < n; ++j) {
661 for (i = j + 1; i < n; ++i) {
662 int idx_ik = k * n + i;
663 int idx_kj = j * n + k;
664 const unsigned sum = adj[idx_ik] + adj[idx_kj];
665 int idx_ij = j * n + i;
666 if (adj[idx_ij] > sum) {
667 int idx_ji = j * n + i;
668 adj[idx_ij] = adj[idx_ji] = sum;
669 }
670 }
671 }
672 }
673 }
674
675 // Build links.
676 for (j = 0; j < n; ++j) {
677 for (i = j + 1; i < n; ++i) {
678 int idx_ij = j * n + i;
679 if (adj[idx_ij] == (unsigned)p_distance) {
680 append_link(i, j);
681 }
682 }
683 }
684 memdelete_arr(adj);
685 }
686}
687
688//===================================================================
689//
690//
691// This function takes in a list of interdependent Links and tries
692// to maximize the distance between calculation
693// of dependent links. This increases the amount of parallelism that can
694// be exploited by out-of-order instruction processors with large but
695// (inevitably) finite instruction windows.
696//
697//===================================================================
698
699// A small structure to track lists of dependent link calculations.
700class LinkDeps {
701public:
702 // A link calculation that is dependent on this one.
703 // Positive values = "input A" while negative values = "input B".
704 int value;
705 // Next dependence in the list.
706 LinkDeps *next;
707};
708typedef LinkDeps *LinkDepsPtr;
709
710void GodotSoftBody3D::reoptimize_link_order() {
711 const int reop_not_dependent = -1;
712 const int reop_node_complete = -2;
713
714 uint32_t link_count = links.size();
715 uint32_t node_count = nodes.size();
716
717 if (link_count < 1 || node_count < 2) {
718 return;
719 }
720
721 uint32_t i;
722 Link *lr;
723 int ar, br;
724 Node *node0 = &(nodes[0]);
725 Node *node1 = &(nodes[1]);
726 LinkDepsPtr link_dep;
727 int ready_list_head, ready_list_tail, link_num, link_dep_frees, dep_link;
728
729 // Allocate temporary buffers.
730 int *node_written_at = memnew_arr(int, node_count + 1); // What link calculation produced this node's current values?
731 int *link_dep_A = memnew_arr(int, link_count); // Link calculation input is dependent upon prior calculation #N
732 int *link_dep_B = memnew_arr(int, link_count);
733 int *ready_list = memnew_arr(int, link_count); // List of ready-to-process link calculations (# of links, maximum)
734 LinkDeps *link_dep_free_list = memnew_arr(LinkDeps, 2 * link_count); // Dependent-on-me list elements (2x# of links, maximum)
735 LinkDepsPtr *link_dep_list_starts = memnew_arr(LinkDepsPtr, link_count); // Start nodes of dependent-on-me lists, one for each link
736
737 // Copy the original, unsorted links to a side buffer.
738 Link *link_buffer = memnew_arr(Link, link_count);
739 memcpy(link_buffer, &(links[0]), sizeof(Link) * link_count);
740
741 // Clear out the node setup and ready list.
742 for (i = 0; i < node_count + 1; i++) {
743 node_written_at[i] = reop_not_dependent;
744 }
745 for (i = 0; i < link_count; i++) {
746 link_dep_list_starts[i] = nullptr;
747 }
748 ready_list_head = ready_list_tail = link_dep_frees = 0;
749
750 // Initial link analysis to set up data structures.
751 for (i = 0; i < link_count; i++) {
752 // Note which prior link calculations we are dependent upon & build up dependence lists.
753 lr = &(links[i]);
754 ar = (lr->n[0] - node0) / (node1 - node0);
755 br = (lr->n[1] - node0) / (node1 - node0);
756 if (node_written_at[ar] > reop_not_dependent) {
757 link_dep_A[i] = node_written_at[ar];
758 link_dep = &link_dep_free_list[link_dep_frees++];
759 link_dep->value = i;
760 link_dep->next = link_dep_list_starts[node_written_at[ar]];
761 link_dep_list_starts[node_written_at[ar]] = link_dep;
762 } else {
763 link_dep_A[i] = reop_not_dependent;
764 }
765 if (node_written_at[br] > reop_not_dependent) {
766 link_dep_B[i] = node_written_at[br];
767 link_dep = &link_dep_free_list[link_dep_frees++];
768 link_dep->value = -(int)(i + 1);
769 link_dep->next = link_dep_list_starts[node_written_at[br]];
770 link_dep_list_starts[node_written_at[br]] = link_dep;
771 } else {
772 link_dep_B[i] = reop_not_dependent;
773 }
774
775 // Add this link to the initial ready list, if it is not dependent on any other links.
776 if ((link_dep_A[i] == reop_not_dependent) && (link_dep_B[i] == reop_not_dependent)) {
777 ready_list[ready_list_tail++] = i;
778 link_dep_A[i] = link_dep_B[i] = reop_node_complete; // Probably not needed now.
779 }
780
781 // Update the nodes to mark which ones are calculated by this link.
782 node_written_at[ar] = node_written_at[br] = i;
783 }
784
785 // Process the ready list and create the sorted list of links:
786 // -- By treating the ready list as a queue, we maximize the distance between any
787 // inter-dependent node calculations.
788 // -- All other (non-related) nodes in the ready list will automatically be inserted
789 // in between each set of inter-dependent link calculations by this loop.
790 i = 0;
791 while (ready_list_head != ready_list_tail) {
792 // Use ready list to select the next link to process.
793 link_num = ready_list[ready_list_head++];
794 // Copy the next-to-calculate link back into the original link array.
795 links[i++] = link_buffer[link_num];
796
797 // Free up any link inputs that are dependent on this one.
798 link_dep = link_dep_list_starts[link_num];
799 while (link_dep) {
800 dep_link = link_dep->value;
801 if (dep_link >= 0) {
802 link_dep_A[dep_link] = reop_not_dependent;
803 } else {
804 dep_link = -dep_link - 1;
805 link_dep_B[dep_link] = reop_not_dependent;
806 }
807 // Add this dependent link calculation to the ready list if *both* inputs are clear.
808 if ((link_dep_A[dep_link] == reop_not_dependent) && (link_dep_B[dep_link] == reop_not_dependent)) {
809 ready_list[ready_list_tail++] = dep_link;
810 link_dep_A[dep_link] = link_dep_B[dep_link] = reop_node_complete; // Probably not needed now.
811 }
812 link_dep = link_dep->next;
813 }
814 }
815
816 // Delete the temporary buffers.
817 memdelete_arr(node_written_at);
818 memdelete_arr(link_dep_A);
819 memdelete_arr(link_dep_B);
820 memdelete_arr(ready_list);
821 memdelete_arr(link_dep_free_list);
822 memdelete_arr(link_dep_list_starts);
823 memdelete_arr(link_buffer);
824}
825
826void GodotSoftBody3D::append_link(uint32_t p_node1, uint32_t p_node2) {
827 if (p_node1 == p_node2) {
828 return;
829 }
830
831 Node *node1 = &nodes[p_node1];
832 Node *node2 = &nodes[p_node2];
833
834 Link link;
835 link.n[0] = node1;
836 link.n[1] = node2;
837 link.rl = (node1->x - node2->x).length();
838
839 links.push_back(link);
840}
841
842void GodotSoftBody3D::append_face(uint32_t p_node1, uint32_t p_node2, uint32_t p_node3) {
843 if (p_node1 == p_node2) {
844 return;
845 }
846 if (p_node1 == p_node3) {
847 return;
848 }
849 if (p_node2 == p_node3) {
850 return;
851 }
852
853 Node *node1 = &nodes[p_node1];
854 Node *node2 = &nodes[p_node2];
855 Node *node3 = &nodes[p_node3];
856
857 Face face;
858 face.n[0] = node1;
859 face.n[1] = node2;
860 face.n[2] = node3;
861
862 face.index = faces.size();
863
864 faces.push_back(face);
865}
866
867void GodotSoftBody3D::set_iteration_count(int p_val) {
868 iteration_count = p_val;
869}
870
871void GodotSoftBody3D::set_total_mass(real_t p_val) {
872 ERR_FAIL_COND(p_val < 0.0);
873
874 inv_total_mass = 1.0 / p_val;
875 real_t mass_factor = total_mass * inv_total_mass;
876 total_mass = p_val;
877
878 uint32_t node_count = nodes.size();
879 for (uint32_t node_index = 0; node_index < node_count; ++node_index) {
880 Node &node = nodes[node_index];
881 node.im *= mass_factor;
882 }
883
884 update_constants();
885}
886
887void GodotSoftBody3D::set_collision_margin(real_t p_val) {
888 collision_margin = p_val;
889}
890
891void GodotSoftBody3D::set_linear_stiffness(real_t p_val) {
892 linear_stiffness = p_val;
893}
894
895void GodotSoftBody3D::set_pressure_coefficient(real_t p_val) {
896 pressure_coefficient = p_val;
897}
898
899void GodotSoftBody3D::set_damping_coefficient(real_t p_val) {
900 damping_coefficient = p_val;
901}
902
903void GodotSoftBody3D::set_drag_coefficient(real_t p_val) {
904 drag_coefficient = p_val;
905}
906
907void GodotSoftBody3D::add_velocity(const Vector3 &p_velocity) {
908 for (Node &node : nodes) {
909 if (node.im > 0) {
910 node.v += p_velocity;
911 }
912 }
913}
914
915void GodotSoftBody3D::apply_forces(const LocalVector<GodotArea3D *> &p_wind_areas) {
916 if (nodes.is_empty()) {
917 return;
918 }
919
920 int32_t j;
921
922 real_t volume = 0.0;
923 const Vector3 &org = nodes[0].x;
924
925 // Iterate over faces (try not to iterate elsewhere if possible).
926 for (const Face &face : faces) {
927 Vector3 wind_force(0, 0, 0);
928
929 // Compute volume.
930 volume += vec3_dot(face.n[0]->x - org, vec3_cross(face.n[1]->x - org, face.n[2]->x - org));
931
932 // Compute nodal forces from area winds.
933 if (!p_wind_areas.is_empty()) {
934 for (const GodotArea3D *area : p_wind_areas) {
935 wind_force += _compute_area_windforce(area, &face);
936 }
937
938 for (j = 0; j < 3; j++) {
939 Node *current_node = face.n[j];
940 current_node->f += wind_force;
941 }
942 }
943 }
944 volume /= 6.0;
945
946 // Apply nodal pressure forces.
947 if (pressure_coefficient > CMP_EPSILON) {
948 real_t ivolumetp = 1.0 / Math::abs(volume) * pressure_coefficient;
949 for (Node &node : nodes) {
950 if (node.im > 0) {
951 node.f += node.n * (node.area * ivolumetp);
952 }
953 }
954 }
955}
956
957Vector3 GodotSoftBody3D::_compute_area_windforce(const GodotArea3D *p_area, const Face *p_face) {
958 real_t wfm = p_area->get_wind_force_magnitude();
959 real_t waf = p_area->get_wind_attenuation_factor();
960 const Vector3 &wd = p_area->get_wind_direction();
961 const Vector3 &ws = p_area->get_wind_source();
962 real_t projection_on_tri_normal = vec3_dot(p_face->normal, wd);
963 real_t projection_toward_centroid = vec3_dot(p_face->centroid - ws, wd);
964 real_t attenuation_over_distance = pow(projection_toward_centroid, -waf);
965 real_t nodal_force_magnitude = wfm * 0.33333333333 * p_face->ra * projection_on_tri_normal * attenuation_over_distance;
966 return nodal_force_magnitude * p_face->normal;
967}
968
969void GodotSoftBody3D::predict_motion(real_t p_delta) {
970 const real_t inv_delta = 1.0 / p_delta;
971
972 ERR_FAIL_COND(!get_space());
973
974 bool gravity_done = false;
975 Vector3 gravity;
976
977 LocalVector<GodotArea3D *> wind_areas;
978
979 int ac = areas.size();
980 if (ac) {
981 areas.sort();
982 const AreaCMP *aa = &areas[0];
983 for (int i = ac - 1; i >= 0; i--) {
984 if (!gravity_done) {
985 PhysicsServer3D::AreaSpaceOverrideMode area_gravity_mode = (PhysicsServer3D::AreaSpaceOverrideMode)(int)aa[i].area->get_param(PhysicsServer3D::AREA_PARAM_GRAVITY_OVERRIDE_MODE);
986 if (area_gravity_mode != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) {
987 Vector3 area_gravity;
988 aa[i].area->compute_gravity(get_transform().get_origin(), area_gravity);
989 switch (area_gravity_mode) {
990 case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE:
991 case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: {
992 gravity += area_gravity;
993 gravity_done = area_gravity_mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE;
994 } break;
995 case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE:
996 case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: {
997 gravity = area_gravity;
998 gravity_done = area_gravity_mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE;
999 } break;
1000 default: {
1001 }
1002 }
1003 }
1004 }
1005
1006 if (aa[i].area->get_wind_force_magnitude() > CMP_EPSILON) {
1007 wind_areas.push_back(aa[i].area);
1008 }
1009 }
1010 }
1011
1012 // Add default gravity and damping from space area.
1013 if (!gravity_done) {
1014 GodotArea3D *default_area = get_space()->get_default_area();
1015 ERR_FAIL_COND(!default_area);
1016
1017 Vector3 default_gravity;
1018 default_area->compute_gravity(get_transform().get_origin(), default_gravity);
1019 gravity += default_gravity;
1020 }
1021
1022 // Apply forces.
1023 add_velocity(gravity * p_delta);
1024 if (pressure_coefficient > CMP_EPSILON || !wind_areas.is_empty()) {
1025 apply_forces(wind_areas);
1026 }
1027
1028 // Avoid soft body from 'exploding' so use some upper threshold of maximum motion
1029 // that a node can travel per frame.
1030 const real_t max_displacement = 1000.0;
1031 real_t clamp_delta_v = max_displacement * inv_delta;
1032
1033 // Integrate.
1034 for (Node &node : nodes) {
1035 node.q = node.x;
1036 Vector3 delta_v = node.f * node.im * p_delta;
1037 for (int c = 0; c < 3; c++) {
1038 delta_v[c] = CLAMP(delta_v[c], -clamp_delta_v, clamp_delta_v);
1039 }
1040 node.v += delta_v;
1041 node.x += node.v * p_delta;
1042 node.f = Vector3();
1043 }
1044
1045 // Bounds and tree update.
1046 update_bounds();
1047
1048 // Node tree update.
1049 for (const Node &node : nodes) {
1050 AABB node_aabb(node.x, Vector3());
1051 node_aabb.expand_to(node.x + node.v * p_delta);
1052 node_aabb.grow_by(collision_margin);
1053
1054 node_tree.update(node.leaf, node_aabb);
1055 }
1056
1057 // Face tree update.
1058 if (!face_tree.is_empty()) {
1059 update_face_tree(p_delta);
1060 }
1061
1062 // Optimize node tree.
1063 node_tree.optimize_incremental(1);
1064 face_tree.optimize_incremental(1);
1065}
1066
1067void GodotSoftBody3D::solve_constraints(real_t p_delta) {
1068 const real_t inv_delta = 1.0 / p_delta;
1069
1070 for (Link &link : links) {
1071 link.c3 = link.n[1]->q - link.n[0]->q;
1072 link.c2 = 1 / (link.c3.length_squared() * link.c0);
1073 }
1074
1075 // Solve velocities.
1076 for (Node &node : nodes) {
1077 node.x = node.q + node.v * p_delta;
1078 }
1079
1080 // Solve positions.
1081 for (int isolve = 0; isolve < iteration_count; ++isolve) {
1082 const real_t ti = isolve / (real_t)iteration_count;
1083 solve_links(1.0, ti);
1084 }
1085 const real_t vc = (1.0 - damping_coefficient) * inv_delta;
1086 for (Node &node : nodes) {
1087 node.x += node.bv * p_delta;
1088 node.bv = Vector3();
1089
1090 node.v = (node.x - node.q) * vc;
1091
1092 node.q = node.x;
1093 }
1094
1095 update_normals_and_centroids();
1096}
1097
1098void GodotSoftBody3D::solve_links(real_t kst, real_t ti) {
1099 for (Link &link : links) {
1100 if (link.c0 > 0) {
1101 Node &node_a = *link.n[0];
1102 Node &node_b = *link.n[1];
1103 const Vector3 del = node_b.x - node_a.x;
1104 const real_t len = del.length_squared();
1105 if (link.c1 + len > CMP_EPSILON) {
1106 const real_t k = ((link.c1 - len) / (link.c0 * (link.c1 + len))) * kst;
1107 node_a.x -= del * (k * node_a.im);
1108 node_b.x += del * (k * node_b.im);
1109 }
1110 }
1111 }
1112}
1113
1114struct AABBQueryResult {
1115 const GodotSoftBody3D *soft_body = nullptr;
1116 void *userdata = nullptr;
1117 GodotSoftBody3D::QueryResultCallback result_callback = nullptr;
1118
1119 _FORCE_INLINE_ bool operator()(void *p_data) {
1120 return result_callback(soft_body->get_node_index(p_data), userdata);
1121 };
1122};
1123
1124void GodotSoftBody3D::query_aabb(const AABB &p_aabb, GodotSoftBody3D::QueryResultCallback p_result_callback, void *p_userdata) {
1125 AABBQueryResult query_result;
1126 query_result.soft_body = this;
1127 query_result.result_callback = p_result_callback;
1128 query_result.userdata = p_userdata;
1129
1130 node_tree.aabb_query(p_aabb, query_result);
1131}
1132
1133struct RayQueryResult {
1134 const GodotSoftBody3D *soft_body = nullptr;
1135 void *userdata = nullptr;
1136 GodotSoftBody3D::QueryResultCallback result_callback = nullptr;
1137
1138 _FORCE_INLINE_ bool operator()(void *p_data) {
1139 return result_callback(soft_body->get_face_index(p_data), userdata);
1140 };
1141};
1142
1143void GodotSoftBody3D::query_ray(const Vector3 &p_from, const Vector3 &p_to, GodotSoftBody3D::QueryResultCallback p_result_callback, void *p_userdata) {
1144 if (face_tree.is_empty()) {
1145 initialize_face_tree();
1146 }
1147
1148 RayQueryResult query_result;
1149 query_result.soft_body = this;
1150 query_result.result_callback = p_result_callback;
1151 query_result.userdata = p_userdata;
1152
1153 face_tree.ray_query(p_from, p_to, query_result);
1154}
1155
1156void GodotSoftBody3D::initialize_face_tree() {
1157 face_tree.clear();
1158 for (Face &face : faces) {
1159 AABB face_aabb;
1160
1161 face_aabb.position = face.n[0]->x;
1162 face_aabb.expand_to(face.n[1]->x);
1163 face_aabb.expand_to(face.n[2]->x);
1164
1165 face_aabb.grow_by(collision_margin);
1166
1167 face.leaf = face_tree.insert(face_aabb, &face);
1168 }
1169}
1170
1171void GodotSoftBody3D::update_face_tree(real_t p_delta) {
1172 for (const Face &face : faces) {
1173 AABB face_aabb;
1174
1175 const Node *node0 = face.n[0];
1176 face_aabb.position = node0->x;
1177 face_aabb.expand_to(node0->x + node0->v * p_delta);
1178
1179 const Node *node1 = face.n[1];
1180 face_aabb.expand_to(node1->x);
1181 face_aabb.expand_to(node1->x + node1->v * p_delta);
1182
1183 const Node *node2 = face.n[2];
1184 face_aabb.expand_to(node2->x);
1185 face_aabb.expand_to(node2->x + node2->v * p_delta);
1186
1187 face_aabb.grow_by(collision_margin);
1188
1189 face_tree.update(face.leaf, face_aabb);
1190 }
1191}
1192
1193void GodotSoftBody3D::initialize_shape(bool p_force_move) {
1194 if (get_shape_count() == 0) {
1195 GodotSoftBodyShape3D *soft_body_shape = memnew(GodotSoftBodyShape3D(this));
1196 add_shape(soft_body_shape);
1197 } else if (p_force_move) {
1198 GodotSoftBodyShape3D *soft_body_shape = static_cast<GodotSoftBodyShape3D *>(get_shape(0));
1199 soft_body_shape->update_bounds();
1200 }
1201}
1202
1203void GodotSoftBody3D::deinitialize_shape() {
1204 if (get_shape_count() > 0) {
1205 GodotShape3D *shape = get_shape(0);
1206 remove_shape(shape);
1207 memdelete(shape);
1208 }
1209}
1210
1211void GodotSoftBody3D::destroy() {
1212 soft_mesh = RID();
1213
1214 map_visual_to_physics.clear();
1215
1216 node_tree.clear();
1217 face_tree.clear();
1218
1219 nodes.clear();
1220 links.clear();
1221 faces.clear();
1222
1223 bounds = AABB();
1224 deinitialize_shape();
1225}
1226
1227void GodotSoftBodyShape3D::update_bounds() {
1228 ERR_FAIL_COND(!soft_body);
1229
1230 AABB collision_aabb = soft_body->get_bounds();
1231 collision_aabb.grow_by(soft_body->get_collision_margin());
1232 configure(collision_aabb);
1233}
1234
1235GodotSoftBodyShape3D::GodotSoftBodyShape3D(GodotSoftBody3D *p_soft_body) {
1236 soft_body = p_soft_body;
1237 update_bounds();
1238}
1239
1240struct _SoftBodyIntersectSegmentInfo {
1241 const GodotSoftBody3D *soft_body = nullptr;
1242 Vector3 from;
1243 Vector3 dir;
1244 Vector3 hit_position;
1245 uint32_t hit_face_index = -1;
1246 real_t hit_dist_sq = INFINITY;
1247
1248 static bool process_hit(uint32_t p_face_index, void *p_userdata) {
1249 _SoftBodyIntersectSegmentInfo &query_info = *(static_cast<_SoftBodyIntersectSegmentInfo *>(p_userdata));
1250
1251 Vector3 points[3];
1252 query_info.soft_body->get_face_points(p_face_index, points[0], points[1], points[2]);
1253
1254 Vector3 result;
1255 if (Geometry3D::ray_intersects_triangle(query_info.from, query_info.dir, points[0], points[1], points[2], &result)) {
1256 real_t dist_sq = query_info.from.distance_squared_to(result);
1257 if (dist_sq < query_info.hit_dist_sq) {
1258 query_info.hit_dist_sq = dist_sq;
1259 query_info.hit_position = result;
1260 query_info.hit_face_index = p_face_index;
1261 }
1262 }
1263
1264 // Continue with the query.
1265 return false;
1266 }
1267};
1268
1269bool GodotSoftBodyShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const {
1270 _SoftBodyIntersectSegmentInfo query_info;
1271 query_info.soft_body = soft_body;
1272 query_info.from = p_begin;
1273 query_info.dir = (p_end - p_begin).normalized();
1274
1275 soft_body->query_ray(p_begin, p_end, _SoftBodyIntersectSegmentInfo::process_hit, &query_info);
1276
1277 if (query_info.hit_dist_sq != INFINITY) {
1278 r_result = query_info.hit_position;
1279 r_normal = soft_body->get_face_normal(query_info.hit_face_index);
1280 return true;
1281 }
1282
1283 return false;
1284}
1285
1286bool GodotSoftBodyShape3D::intersect_point(const Vector3 &p_point) const {
1287 return false;
1288}
1289
1290Vector3 GodotSoftBodyShape3D::get_closest_point_to(const Vector3 &p_point) const {
1291 ERR_FAIL_V_MSG(Vector3(), "Get closest point is not supported for soft bodies.");
1292}
1293