Merge pull request #50785 from jeffrey-cochran/softbody-areas

Updated softbody handling to allow for area/softbody collision detection and application of area gravity

servers/physics_3d/area_3d_sw.cpp

@@ -30,8 +30,16 @@
 
 #include "area_3d_sw.h"
 #include "body_3d_sw.h"
+#include "soft_body_3d_sw.h"
 #include "space_3d_sw.h"
 
+Area3DSW::BodyKey::BodyKey(SoftBody3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) {
+	rid = p_body->get_self();
+	instance_id = p_body->get_instance_id();
+	body_shape = p_body_shape;
+	area_shape = p_area_shape;
+}
+
 Area3DSW::BodyKey::BodyKey(Body3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) {
 	rid = p_body->get_self();
 	instance_id = p_body->get_instance_id();

servers/physics_3d/area_3d_sw.h

@@ -38,6 +38,7 @@
 
 class Space3DSW;
 class Body3DSW;
+class SoftBody3DSW;
 class Constraint3DSW;
 
 class Area3DSW : public CollisionObject3DSW {
@@ -80,6 +81,7 @@ class Area3DSW : public CollisionObject3DSW {
 		}
 
 		_FORCE_INLINE_ BodyKey() {}
+		BodyKey(SoftBody3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape);
 		BodyKey(Body3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape);
 		BodyKey(Area3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape);
 	};
@@ -91,6 +93,7 @@ class Area3DSW : public CollisionObject3DSW {
 		_FORCE_INLINE_ BodyState() { state = 0; }
 	};
 
+	Map<BodyKey, BodyState> monitored_soft_bodies;
 	Map<BodyKey, BodyState> monitored_bodies;
 	Map<BodyKey, BodyState> monitored_areas;
 
@@ -115,6 +118,9 @@ public:
 	_FORCE_INLINE_ void add_body_to_query(Body3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape);
 	_FORCE_INLINE_ void remove_body_from_query(Body3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape);
 
+	_FORCE_INLINE_ void add_soft_body_to_query(SoftBody3DSW *p_soft_body, uint32_t p_soft_body_shape, uint32_t p_area_shape);
+	_FORCE_INLINE_ void remove_soft_body_from_query(SoftBody3DSW *p_soft_body, uint32_t p_soft_body_shape, uint32_t p_area_shape);
+
 	_FORCE_INLINE_ void add_area_to_query(Area3DSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape);
 	_FORCE_INLINE_ void remove_area_from_query(Area3DSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape);
 
@@ -166,6 +172,22 @@ public:
 	~Area3DSW();
 };
 
+void Area3DSW::add_soft_body_to_query(SoftBody3DSW *p_soft_body, uint32_t p_soft_body_shape, uint32_t p_area_shape) {
+	BodyKey bk(p_soft_body, p_soft_body_shape, p_area_shape);
+	monitored_soft_bodies[bk].inc();
+	if (!monitor_query_list.in_list()) {
+		_queue_monitor_update();
+	}
+}
+
+void Area3DSW::remove_soft_body_from_query(SoftBody3DSW *p_soft_body, uint32_t p_soft_body_shape, uint32_t p_area_shape) {
+	BodyKey bk(p_soft_body, p_soft_body_shape, p_area_shape);
+	monitored_soft_bodies[bk].dec();
+	if (!monitor_query_list.in_list()) {
+		_queue_monitor_update();
+	}
+}
+
 void Area3DSW::add_body_to_query(Body3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) {
 	BodyKey bk(p_body, p_body_shape, p_area_shape);
 	monitored_bodies[bk].inc();
@@ -198,4 +220,16 @@ void Area3DSW::remove_area_from_query(Area3DSW *p_area, uint32_t p_area_shape, u
 	}
 }
 
+struct AreaCMP {
+	Area3DSW *area;
+	int refCount;
+	_FORCE_INLINE_ bool operator==(const AreaCMP &p_cmp) const { return area->get_self() == p_cmp.area->get_self(); }
+	_FORCE_INLINE_ bool operator<(const AreaCMP &p_cmp) const { return area->get_priority() < p_cmp.area->get_priority(); }
+	_FORCE_INLINE_ AreaCMP() {}
+	_FORCE_INLINE_ AreaCMP(Area3DSW *p_area) {
+		area = p_area;
+		refCount = 1;
+	}
+};
+
 #endif // AREA__SW_H

servers/physics_3d/area_pair_3d_sw.cpp

@@ -181,3 +181,85 @@ Area2Pair3DSW::~Area2Pair3DSW() {
 	area_a->remove_constraint(this);
 	area_b->remove_constraint(this);
 }
+
+////////////////////////////////////////////////////
+
+bool AreaSoftBodyPair3DSW::setup(real_t p_step) {
+	bool result = false;
+	if (
+			area->interacts_with(soft_body) &&
+			CollisionSolver3DSW::solve_static(
+					soft_body->get_shape(soft_body_shape),
+					soft_body->get_transform() * soft_body->get_shape_transform(soft_body_shape),
+					area->get_shape(area_shape),
+					area->get_transform() * area->get_shape_transform(area_shape),
+					nullptr,
+					this)) {
+		result = true;
+	}
+
+	process_collision = false;
+	if (result != colliding) {
+		if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) {
+			process_collision = true;
+		} else if (area->has_monitor_callback()) {
+			process_collision = true;
+		}
+
+		colliding = result;
+	}
+
+	return process_collision;
+}
+
+bool AreaSoftBodyPair3DSW::pre_solve(real_t p_step) {
+	if (!process_collision) {
+		return false;
+	}
+
+	if (colliding) {
+		if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) {
+			soft_body->add_area(area);
+		}
+
+		if (area->has_monitor_callback()) {
+			area->add_soft_body_to_query(soft_body, soft_body_shape, area_shape);
+		}
+	} else {
+		if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) {
+			soft_body->remove_area(area);
+		}
+
+		if (area->has_monitor_callback()) {
+			area->remove_soft_body_from_query(soft_body, soft_body_shape, area_shape);
+		}
+	}
+
+	return false; // Never do any post solving.
+}
+
+void AreaSoftBodyPair3DSW::solve(real_t p_step) {
+	// Nothing to do.
+}
+
+AreaSoftBodyPair3DSW::AreaSoftBodyPair3DSW(SoftBody3DSW *p_soft_body, int p_soft_body_shape, Area3DSW *p_area, int p_area_shape) {
+	soft_body = p_soft_body;
+	area = p_area;
+	soft_body_shape = p_soft_body_shape;
+	area_shape = p_area_shape;
+	soft_body->add_constraint(this);
+	area->add_constraint(this);
+}
+
+AreaSoftBodyPair3DSW::~AreaSoftBodyPair3DSW() {
+	if (colliding) {
+		if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) {
+			soft_body->remove_area(area);
+		}
+		if (area->has_monitor_callback()) {
+			area->remove_soft_body_from_query(soft_body, soft_body_shape, area_shape);
+		}
+	}
+	soft_body->remove_constraint(this);
+	area->remove_constraint(this);
+}

servers/physics_3d/area_pair_3d_sw.h

@@ -34,6 +34,7 @@
 #include "area_3d_sw.h"
 #include "body_3d_sw.h"
 #include "constraint_3d_sw.h"
+#include "soft_body_3d_sw.h"
 
 class AreaPair3DSW : public Constraint3DSW {
 	Body3DSW *body;
@@ -69,4 +70,21 @@ public:
 	~Area2Pair3DSW();
 };
 
+class AreaSoftBodyPair3DSW : public Constraint3DSW {
+	SoftBody3DSW *soft_body;
+	Area3DSW *area;
+	int soft_body_shape;
+	int area_shape;
+	bool colliding = false;
+	bool process_collision = false;
+
+public:
+	virtual bool setup(real_t p_step) override;
+	virtual bool pre_solve(real_t p_step) override;
+	virtual void solve(real_t p_step) override;
+
+	AreaSoftBodyPair3DSW(SoftBody3DSW *p_sof_body, int p_soft_body_shape, Area3DSW *p_area, int p_area_shape);
+	~AreaSoftBodyPair3DSW();
+};
+
 #endif // AREA_PAIR__SW_H

servers/physics_3d/body_3d_sw.h

@@ -96,18 +96,6 @@ class Body3DSW : public CollisionObject3DSW {
 
 	Map<Constraint3DSW *, int> constraint_map;
 
-	struct AreaCMP {
-		Area3DSW *area;
-		int refCount;
-		_FORCE_INLINE_ bool operator==(const AreaCMP &p_cmp) const { return area->get_self() == p_cmp.area->get_self(); }
-		_FORCE_INLINE_ bool operator<(const AreaCMP &p_cmp) const { return area->get_priority() < p_cmp.area->get_priority(); }
-		_FORCE_INLINE_ AreaCMP() {}
-		_FORCE_INLINE_ AreaCMP(Area3DSW *p_area) {
-			area = p_area;
-			refCount = 1;
-		}
-	};
-
 	Vector<AreaCMP> areas;
 
 	struct Contact {

servers/physics_3d/collision_solver_3d_sw.cpp

@@ -102,6 +102,10 @@ void CollisionSolver3DSW::soft_body_contact_callback(const Vector3 &p_point_A, i
 
 	++cinfo.contact_count;
 
+	if (!cinfo.result_callback) {
+		return;
+	}
+
 	if (cinfo.swap_result) {
 		cinfo.result_callback(p_point_B, cinfo.node_index, p_point_A, p_index_A, cinfo.userdata);
 	} else {

servers/physics_3d/soft_body_3d_sw.cpp

@@ -928,6 +928,19 @@ void SoftBody3DSW::apply_forces() {
 	}
 }
 
+void SoftBody3DSW::_compute_area_gravity(const Area3DSW *p_area) {
+	if (p_area->is_gravity_point()) {
+		if (p_area->get_gravity_distance_scale() > 0) {
+			Vector3 v = p_area->get_transform().xform(p_area->get_gravity_vector()) - get_transform().get_origin();
+			gravity += v.normalized() * (p_area->get_gravity() / Math::pow(v.length() * p_area->get_gravity_distance_scale() + 1, 2));
+		} else {
+			gravity += (p_area->get_transform().xform(p_area->get_gravity_vector()) - get_transform().get_origin()).normalized() * p_area->get_gravity();
+		}
+	} else {
+		gravity += p_area->get_gravity_vector() * p_area->get_gravity();
+	}
+}
+
 void SoftBody3DSW::predict_motion(real_t p_delta) {
 	const real_t inv_delta = 1.0 / p_delta;
 
@@ -935,9 +948,35 @@ void SoftBody3DSW::predict_motion(real_t p_delta) {
 
 	Area3DSW *def_area = get_space()->get_default_area();
 	ERR_FAIL_COND(!def_area);
+	gravity = def_area->get_gravity_vector() * def_area->get_gravity();
+
+	int ac = areas.size();
+	bool stopped = false;
+
+	if (ac) {
+		areas.sort();
+		const AreaCMP *aa = &areas[0];
+		for (int i = ac - 1; i >= 0 && !stopped; i--) {
+			PhysicsServer3D::AreaSpaceOverrideMode mode = aa[i].area->get_space_override_mode();
+			switch (mode) {
+				case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE:
+				case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: {
+					_compute_area_gravity(aa[i].area);
+					stopped = mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE;
+				} break;
+				case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE:
+				case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: {
+					gravity = Vector3(0, 0, 0);
+					_compute_area_gravity(aa[i].area);
+					stopped = mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE;
+				} break;
+				default: {
+				}
+			}
+		}
+	}
 
 	// Apply forces.
-	Vector3 gravity = def_area->get_gravity_vector() * def_area->get_gravity();
 	add_velocity(gravity * p_delta);
 	apply_forces();
 

servers/physics_3d/soft_body_3d_sw.h

@@ -31,6 +31,7 @@
 #ifndef SOFT_BODY_3D_SW_H
 #define SOFT_BODY_3D_SW_H
 
+#include "area_3d_sw.h"
 #include "collision_object_3d_sw.h"
 
 #include "core/math/aabb.h"
@@ -100,14 +101,20 @@ class SoftBody3DSW : public CollisionObject3DSW {
 	real_t drag_coefficient = 0.0; // [0,1]
 	LocalVector<int> pinned_vertices;
 
+	Vector3 gravity;
+
 	SelfList<SoftBody3DSW> active_list;
 
 	Set<Constraint3DSW *> constraints;
 
+	Vector<AreaCMP> areas;
+
 	VSet<RID> exceptions;
 
 	uint64_t island_step = 0;
 
+	_FORCE_INLINE_ void _compute_area_gravity(const Area3DSW *p_area);
+
 public:
 	SoftBody3DSW();
 
@@ -129,6 +136,25 @@ public:
 	_FORCE_INLINE_ uint64_t get_island_step() const { return island_step; }
 	_FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; }
 
+	_FORCE_INLINE_ void add_area(Area3DSW *p_area) {
+		int index = areas.find(AreaCMP(p_area));
+		if (index > -1) {
+			areas.write[index].refCount += 1;
+		} else {
+			areas.ordered_insert(AreaCMP(p_area));
+		}
+	}
+
+	_FORCE_INLINE_ void remove_area(Area3DSW *p_area) {
+		int index = areas.find(AreaCMP(p_area));
+		if (index > -1) {
+			areas.write[index].refCount -= 1;
+			if (areas[index].refCount < 1) {
+				areas.remove(index);
+			}
+		}
+	}
+
 	virtual void set_space(Space3DSW *p_space);
 
 	void set_mesh(const Ref<Mesh> &p_mesh);

servers/physics_3d/space_3d_sw.cpp

@@ -921,7 +921,9 @@ void *Space3DSW::_broadphase_pair(CollisionObject3DSW *A, int p_subindex_A, Coll
 			Area2Pair3DSW *area2_pair = memnew(Area2Pair3DSW(area_b, p_subindex_B, area, p_subindex_A));
 			return area2_pair;
 		} else if (type_B == CollisionObject3DSW::TYPE_SOFT_BODY) {
-			// Area/Soft Body, not supported.
+			SoftBody3DSW *softbody = static_cast<SoftBody3DSW *>(B);
+			AreaSoftBodyPair3DSW *soft_area_pair = memnew(AreaSoftBodyPair3DSW(softbody, p_subindex_B, area, p_subindex_A));
+			return soft_area_pair;
 		} else {
 			Body3DSW *body = static_cast<Body3DSW *>(B);
 			AreaPair3DSW *area_pair = memnew(AreaPair3DSW(body, p_subindex_B, area, p_subindex_A));