SoftBody support in GodotPhysics 3D

- Fixed SoftBody surface update with new rendering system - Added GodotPhysics implementation for SoftBody - Added support to get SoftBody rid to interact with the physics server - Added support to get SoftBody bounds from the physics server - Removed support for unused get_vertex_position and get_point_offset from the physics server - Removed SoftBody properties that are unused in both Bullet and GodotPhysics (angular and volume stiffness, pose matching) - Added RenderingServerHandler interface to PhysicsServer3D so the physics servers don't need to reference the class from SoftBody node directly
2021-03-11 20:33:46 -07:00
parent 01851defb5
commit d5ea4acd2d
26 changed files with 2489 additions and 359 deletions
@@ -30,6 +30,7 @@

 #include "collision_solver_3d_sw.h"
 #include "collision_solver_3d_sat.h"
+#include "soft_body_3d_sw.h"

 #include "gjk_epa.h"

@@ -78,9 +79,9 @@ bool CollisionSolver3DSW::solve_static_plane(const Shape3DSW *p_shape_A, const T

 		if (p_result_callback) {
 			if (p_swap_result) {
-				p_result_callback(supports[i], support_A, p_userdata);
+				p_result_callback(supports[i], 0, support_A, 0, p_userdata);
 			} else {
-				p_result_callback(support_A, supports[i], p_userdata);
+				p_result_callback(support_A, 0, supports[i], 0, p_userdata);
 			}
 		}
 	}
@@ -113,14 +114,148 @@ bool CollisionSolver3DSW::solve_ray(const Shape3DSW *p_shape_A, const Transform

 	if (p_result_callback) {
 		if (p_swap_result) {
-			p_result_callback(support_B, support_A, p_userdata);
+			p_result_callback(support_B, 0, support_A, 0, p_userdata);
 		} else {
-			p_result_callback(support_A, support_B, p_userdata);
+			p_result_callback(support_A, 0, support_B, 0, p_userdata);
 		}
 	}
 	return true;
 }

+struct _SoftBodyContactCollisionInfo {
+	int node_index = 0;
+	CollisionSolver3DSW::CallbackResult result_callback = nullptr;
+	void *userdata = nullptr;
+	bool swap_result = false;
+	int contact_count = 0;
+};
+
+void CollisionSolver3DSW::soft_body_contact_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) {
+	_SoftBodyContactCollisionInfo &cinfo = *(_SoftBodyContactCollisionInfo *)(p_userdata);
+
+	++cinfo.contact_count;
+
+	if (cinfo.swap_result) {
+		cinfo.result_callback(p_point_B, cinfo.node_index, p_point_A, p_index_A, cinfo.userdata);
+	} else {
+		cinfo.result_callback(p_point_A, p_index_A, p_point_B, cinfo.node_index, cinfo.userdata);
+	}
+}
+
+struct _SoftBodyQueryInfo {
+	SoftBody3DSW *soft_body = nullptr;
+	const Shape3DSW *shape_A = nullptr;
+	const Shape3DSW *shape_B = nullptr;
+	Transform transform_A;
+	Transform node_transform;
+	_SoftBodyContactCollisionInfo contact_info;
+#ifdef DEBUG_ENABLED
+	int node_query_count = 0;
+	int convex_query_count = 0;
+#endif
+};
+
+bool CollisionSolver3DSW::soft_body_query_callback(uint32_t p_node_index, void *p_userdata) {
+	_SoftBodyQueryInfo &query_cinfo = *(_SoftBodyQueryInfo *)(p_userdata);
+
+	Vector3 node_position = query_cinfo.soft_body->get_node_position(p_node_index);
+
+	Transform transform_B;
+	transform_B.origin = query_cinfo.node_transform.xform(node_position);
+
+	query_cinfo.contact_info.node_index = p_node_index;
+	solve_static(query_cinfo.shape_A, query_cinfo.transform_A, query_cinfo.shape_B, transform_B, soft_body_contact_callback, &query_cinfo.contact_info);
+
+#ifdef DEBUG_ENABLED
+	++query_cinfo.node_query_count;
+#endif
+
+	// Continue with the query.
+	return false;
+}
+
+void CollisionSolver3DSW::soft_body_concave_callback(void *p_userdata, Shape3DSW *p_convex) {
+	_SoftBodyQueryInfo &query_cinfo = *(_SoftBodyQueryInfo *)(p_userdata);
+
+	query_cinfo.shape_A = p_convex;
+
+	// Calculate AABB for internal soft body query (in world space).
+	AABB shape_aabb;
+	for (int i = 0; i < 3; i++) {
+		Vector3 axis;
+		axis[i] = 1.0;
+
+		real_t smin, smax;
+		p_convex->project_range(axis, query_cinfo.transform_A, smin, smax);
+
+		shape_aabb.position[i] = smin;
+		shape_aabb.size[i] = smax - smin;
+	}
+
+	shape_aabb.grow_by(query_cinfo.soft_body->get_collision_margin());
+
+	query_cinfo.soft_body->query_aabb(shape_aabb, soft_body_query_callback, &query_cinfo);
+
+#ifdef DEBUG_ENABLED
+	++query_cinfo.convex_query_count;
+#endif
+}
+
+bool CollisionSolver3DSW::solve_soft_body(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) {
+	const SoftBodyShape3DSW *soft_body_shape_B = static_cast<const SoftBodyShape3DSW *>(p_shape_B);
+
+	SoftBody3DSW *soft_body = soft_body_shape_B->get_soft_body();
+	const Transform &world_to_local = soft_body->get_inv_transform();
+
+	const real_t collision_margin = soft_body->get_collision_margin();
+
+	SphereShape3DSW sphere_shape;
+	sphere_shape.set_data(collision_margin);
+
+	_SoftBodyQueryInfo query_cinfo;
+	query_cinfo.contact_info.result_callback = p_result_callback;
+	query_cinfo.contact_info.userdata = p_userdata;
+	query_cinfo.contact_info.swap_result = p_swap_result;
+	query_cinfo.soft_body = soft_body;
+	query_cinfo.node_transform = p_transform_B * world_to_local;
+	query_cinfo.shape_A = p_shape_A;
+	query_cinfo.transform_A = p_transform_A;
+	query_cinfo.shape_B = &sphere_shape;
+
+	if (p_shape_A->is_concave()) {
+		// In case of concave shape, query convex shapes first.
+		const ConcaveShape3DSW *concave_shape_A = static_cast<const ConcaveShape3DSW *>(p_shape_A);
+
+		AABB soft_body_aabb = soft_body->get_bounds();
+		soft_body_aabb.grow_by(collision_margin);
+
+		// Calculate AABB for internal concave shape query (in local space).
+		AABB local_aabb;
+		for (int i = 0; i < 3; i++) {
+			Vector3 axis(p_transform_A.basis.get_axis(i));
+			real_t axis_scale = 1.0 / axis.length();
+
+			real_t smin = soft_body_aabb.position[i];
+			real_t smax = smin + soft_body_aabb.size[i];
+
+			smin *= axis_scale;
+			smax *= axis_scale;
+
+			local_aabb.position[i] = smin;
+			local_aabb.size[i] = smax - smin;
+		}
+
+		concave_shape_A->cull(local_aabb, soft_body_concave_callback, &query_cinfo);
+	} else {
+		AABB shape_aabb = p_transform_A.xform(p_shape_A->get_aabb());
+		shape_aabb.grow_by(collision_margin);
+
+		soft_body->query_aabb(shape_aabb, soft_body_query_callback, &query_cinfo);
+	}
+
+	return (query_cinfo.contact_info.contact_count > 0);
+}
+
 struct _ConcaveCollisionInfo {
 	const Transform *transform_A;
 	const Shape3DSW *shape_A;
@@ -215,6 +350,9 @@ bool CollisionSolver3DSW::solve_static(const Shape3DSW *p_shape_A, const Transfo
 		if (type_B == PhysicsServer3D::SHAPE_RAY) {
 			return false;
 		}
+		if (type_B == PhysicsServer3D::SHAPE_SOFT_BODY) {
+			return false;
+		}

 		if (swap) {
 			return solve_static_plane(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true);
@@ -233,6 +371,18 @@ bool CollisionSolver3DSW::solve_static(const Shape3DSW *p_shape_A, const Transfo
 			return solve_ray(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false);
 		}

+	} else if (type_B == PhysicsServer3D::SHAPE_SOFT_BODY) {
+		if (type_A == PhysicsServer3D::SHAPE_SOFT_BODY) {
+			// Soft Body / Soft Body not supported.
+			return false;
+		}
+
+		if (swap) {
+			return solve_soft_body(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true);
+		} else {
+			return solve_soft_body(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false);
+		}
+
 	} else if (concave_B) {
 		if (concave_A) {
 			return false;