/*************************************************************************/
/*  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 Transform3D &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 = INFINITY;

	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 != INFINITY) {
		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.");
}