Enabled area-specific wind forces

6 files changed, 120 insertions, 19 deletions

diff --git a/servers/physics_3d/area_3d_sw.cpp b/servers/physics_3d/area_3d_sw.cpp
index 364f63e4ad..c292abad48 100644
--- a/servers/physics_3d/area_3d_sw.cpp
+++ b/servers/physics_3d/area_3d_sw.cpp
@@ -163,6 +163,20 @@ void Area3DSW::set_param(PhysicsServer3D::AreaParameter p_param, const Variant &
 		case PhysicsServer3D::AREA_PARAM_PRIORITY:
 			priority = p_value;
 			break;
+		case PhysicsServer3D::AREA_PARAM_WIND_FORCE_MAGNITUDE:
+			ERR_FAIL_COND_MSG(wind_force_magnitude < 0, "Wind force magnitude must be a non-negative real number, but a negative number was specified.");
+			wind_force_magnitude = p_value;
+			break;
+		case PhysicsServer3D::AREA_PARAM_WIND_SOURCE:
+			wind_source = p_value;
+			break;
+		case PhysicsServer3D::AREA_PARAM_WIND_DIRECTION:
+			wind_direction = p_value;
+			break;
+		case PhysicsServer3D::AREA_PARAM_WIND_ATTENUATION_FACTOR:
+			ERR_FAIL_COND_MSG(wind_attenuation_factor < 0, "Wind attenuation factor must be a non-negative real number, but a negative number was specified.");
+			wind_attenuation_factor = p_value;
+			break;
 	}
 }
 
@@ -184,6 +198,14 @@ Variant Area3DSW::get_param(PhysicsServer3D::AreaParameter p_param) const {
 			return angular_damp;
 		case PhysicsServer3D::AREA_PARAM_PRIORITY:
 			return priority;
+		case PhysicsServer3D::AREA_PARAM_WIND_FORCE_MAGNITUDE:
+			return wind_force_magnitude;
+		case PhysicsServer3D::AREA_PARAM_WIND_SOURCE:
+			return wind_source;
+		case PhysicsServer3D::AREA_PARAM_WIND_DIRECTION:
+			return wind_direction;
+		case PhysicsServer3D::AREA_PARAM_WIND_ATTENUATION_FACTOR:
+			return wind_attenuation_factor;
 	}
 
 	return Variant();
diff --git a/servers/physics_3d/area_3d_sw.h b/servers/physics_3d/area_3d_sw.h
index 5959ee1e95..814472885c 100644
--- a/servers/physics_3d/area_3d_sw.h
+++ b/servers/physics_3d/area_3d_sw.h
@@ -50,6 +50,10 @@ class Area3DSW : public CollisionObject3DSW {
 	real_t point_attenuation;
 	real_t linear_damp;
 	real_t angular_damp;
+	real_t wind_force_magnitude = 0.0;
+	real_t wind_attenuation_factor = 0.0;
+	Vector3 wind_source;
+	Vector3 wind_direction;
 	int priority;
 	bool monitorable;
 
@@ -154,6 +158,18 @@ public:
 	_FORCE_INLINE_ void set_priority(int p_priority) { priority = p_priority; }
 	_FORCE_INLINE_ int get_priority() const { return priority; }
 
+	_FORCE_INLINE_ void set_wind_force_magnitude(real_t p_wind_force_magnitude) { wind_force_magnitude = p_wind_force_magnitude; }
+	_FORCE_INLINE_ real_t get_wind_force_magnitude() const { return wind_force_magnitude; }
+
+	_FORCE_INLINE_ void set_wind_attenuation_factor(real_t p_wind_attenuation_factor) { wind_attenuation_factor = p_wind_attenuation_factor; }
+	_FORCE_INLINE_ real_t get_wind_attenuation_factor() const { return wind_attenuation_factor; }
+
+	_FORCE_INLINE_ void set_wind_source(const Vector3 &p_wind_source) { wind_source = p_wind_source; }
+	_FORCE_INLINE_ const Vector3 &get_wind_source() const { return wind_source; }
+
+	_FORCE_INLINE_ void set_wind_direction(const Vector3 &p_wind_direction) { wind_direction = p_wind_direction; }
+	_FORCE_INLINE_ const Vector3 &get_wind_direction() const { return wind_direction; }
+
 	_FORCE_INLINE_ void add_constraint(Constraint3DSW *p_constraint) { constraints.insert(p_constraint); }
 	_FORCE_INLINE_ void remove_constraint(Constraint3DSW *p_constraint) { constraints.erase(p_constraint); }
 	_FORCE_INLINE_ const Set<Constraint3DSW *> &get_constraints() const { return constraints; }
diff --git a/servers/physics_3d/soft_body_3d_sw.cpp b/servers/physics_3d/soft_body_3d_sw.cpp
index 73b81444e1..bd02d1ed05 100644
--- a/servers/physics_3d/soft_body_3d_sw.cpp
+++ b/servers/physics_3d/soft_body_3d_sw.cpp
@@ -165,7 +165,7 @@ void SoftBody3DSW::update_rendering_server(RenderingServerHandler *p_rendering_s
 	p_rendering_server_handler->set_aabb(bounds);
 }
 
-void SoftBody3DSW::update_normals() {
+void SoftBody3DSW::update_normals_and_centroids() {
 	uint32_t i, ni;
 
 	for (i = 0, ni = nodes.size(); i < ni; ++i) {
@@ -180,6 +180,7 @@ void SoftBody3DSW::update_normals() {
 		face.n[2]->n += n;
 		face.normal = n;
 		face.normal.normalize();
+		face.centroid = 0.33333333333 * (face.n[0]->x + face.n[1]->x + face.n[2]->x);
 	}
 
 	for (i = 0, ni = nodes.size(); i < ni; ++i) {
@@ -310,7 +311,7 @@ void SoftBody3DSW::apply_nodes_transform(const Transform3D &p_transform) {
 
 	face_tree.clear();
 
-	update_normals();
+	update_normals_and_centroids();
 	update_bounds();
 	update_constants();
 }
@@ -574,7 +575,7 @@ bool SoftBody3DSW::create_from_trimesh(const Vector<int> &p_indices, const Vecto
 	reoptimize_link_order();
 
 	update_constants();
-	update_normals();
+	update_normals_and_centroids();
 	update_bounds();
 
 	return true;
@@ -898,32 +899,66 @@ void SoftBody3DSW::add_velocity(const Vector3 &p_velocity) {
 	}
 }
 
-void SoftBody3DSW::apply_forces() {
-	if (pressure_coefficient < CMP_EPSILON) {
-		return;
-	}
+void SoftBody3DSW::apply_forces(bool p_has_wind_forces) {
+	int ac = areas.size();
 
 	if (nodes.is_empty()) {
 		return;
 	}
 
 	uint32_t i, ni;
+	int32_t j;
 
-	// Calculate volume.
 	real_t volume = 0.0;
 	const Vector3 &org = nodes[0].x;
+
+	// Iterate over faces (try not to iterate elsewhere if possible).
 	for (i = 0, ni = faces.size(); i < ni; ++i) {
+		bool stopped = false;
 		const Face &face = faces[i];
+
+		Vector3 wind_force(0, 0, 0);
+
+		// Compute volume.
 		volume += vec3_dot(face.n[0]->x - org, vec3_cross(face.n[1]->x - org, face.n[2]->x - org));
+
+		// Compute nodal forces from area winds.
+		if (ac && p_has_wind_forces) {
+			const AreaCMP *aa = &areas[0];
+			for (j = ac - 1; j >= 0 && !stopped; j--) {
+				PhysicsServer3D::AreaSpaceOverrideMode mode = aa[j].area->get_space_override_mode();
+				switch (mode) {
+					case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE:
+					case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: {
+						wind_force += _compute_area_windforce(aa[j].area, &face);
+						stopped = mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE;
+					} break;
+					case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE:
+					case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: {
+						wind_force = _compute_area_windforce(aa[j].area, &face);
+						stopped = mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE;
+					} break;
+					default: {
+					}
+				}
+			}
+
+			for (j = 0; j < 3; j++) {
+				Node *current_node = face.n[j];
+				current_node->f += wind_force;
+			}
+		}
 	}
 	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);
+	// Apply nodal pressure forces.
+	if (pressure_coefficient > CMP_EPSILON) {
+		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);
+			}
 		}
 	}
 }
@@ -941,6 +976,18 @@ void SoftBody3DSW::_compute_area_gravity(const Area3DSW *p_area) {
 	}
 }
 
+Vector3 SoftBody3DSW::_compute_area_windforce(const Area3DSW *p_area, const Face *p_face) {
+	real_t wfm = p_area->get_wind_force_magnitude();
+	real_t waf = p_area->get_wind_attenuation_factor();
+	const Vector3 &wd = p_area->get_wind_direction();
+	const Vector3 &ws = p_area->get_wind_source();
+	real_t projection_on_tri_normal = vec3_dot(p_face->normal, wd);
+	real_t projection_toward_centroid = vec3_dot(p_face->centroid - ws, wd);
+	real_t attenuation_over_distance = pow(projection_toward_centroid, -waf);
+	real_t nodal_force_magnitude = wfm * 0.33333333333 * p_face->ra * projection_on_tri_normal * attenuation_over_distance;
+	return nodal_force_magnitude * p_face->normal;
+}
+
 void SoftBody3DSW::predict_motion(real_t p_delta) {
 	const real_t inv_delta = 1.0 / p_delta;
 
@@ -952,11 +999,15 @@ void SoftBody3DSW::predict_motion(real_t p_delta) {
 
 	int ac = areas.size();
 	bool stopped = false;
+	bool has_wind_forces = false;
 
 	if (ac) {
 		areas.sort();
 		const AreaCMP *aa = &areas[0];
 		for (int i = ac - 1; i >= 0 && !stopped; i--) {
+			// Avoids unnecessary loop in apply_forces().
+			has_wind_forces = has_wind_forces || aa[i].area->get_wind_force_magnitude() > CMP_EPSILON;
+
 			PhysicsServer3D::AreaSpaceOverrideMode mode = aa[i].area->get_space_override_mode();
 			switch (mode) {
 				case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE:
@@ -978,7 +1029,9 @@ void SoftBody3DSW::predict_motion(real_t p_delta) {
 
 	// Apply forces.
 	add_velocity(gravity * p_delta);
-	apply_forces();
+	if (pressure_coefficient > CMP_EPSILON || has_wind_forces) {
+		apply_forces(has_wind_forces);
+	}
 
 	// Avoid soft body from 'exploding' so use some upper threshold of maximum motion
 	// that a node can travel per frame.
@@ -1057,7 +1110,7 @@ void SoftBody3DSW::solve_constraints(real_t p_delta) {
 		node.q = node.x;
 	}
 
-	update_normals();
+	update_normals_and_centroids();
 }
 
 void SoftBody3DSW::solve_links(real_t kst, real_t ti) {
diff --git a/servers/physics_3d/soft_body_3d_sw.h b/servers/physics_3d/soft_body_3d_sw.h
index be0620c506..58fd234fde 100644
--- a/servers/physics_3d/soft_body_3d_sw.h
+++ b/servers/physics_3d/soft_body_3d_sw.h
@@ -71,6 +71,7 @@ class SoftBody3DSW : public CollisionObject3DSW {
 	};
 
 	struct Face {
+		Vector3 centroid;
 		Node *n[3] = { nullptr, nullptr, nullptr }; // Node pointers
 		Vector3 normal; // Normal
 		real_t ra = 0.0; // Rest area
@@ -114,6 +115,7 @@ class SoftBody3DSW : public CollisionObject3DSW {
 	uint64_t island_step = 0;
 
 	_FORCE_INLINE_ void _compute_area_gravity(const Area3DSW *p_area);
+	_FORCE_INLINE_ Vector3 _compute_area_windforce(const Area3DSW *p_area, const Face *p_face);
 
 public:
 	SoftBody3DSW();
@@ -220,7 +222,7 @@ protected:
 	virtual void _shapes_changed();
 
 private:
-	void update_normals();
+	void update_normals_and_centroids();
 	void update_bounds();
 	void update_constants();
 	void update_area();
@@ -231,7 +233,7 @@ private:
 
 	void add_velocity(const Vector3 &p_velocity);
 
-	void apply_forces();
+	void apply_forces(bool p_has_wind_forces);
 
 	bool create_from_trimesh(const Vector<int> &p_indices, const Vector<Vector3> &p_vertices);
 	void generate_bending_constraints(int p_distance);
diff --git a/servers/physics_server_3d.cpp b/servers/physics_server_3d.cpp
index 53863cea72..1f46a96b27 100644
--- a/servers/physics_server_3d.cpp
+++ b/servers/physics_server_3d.cpp
@@ -766,6 +766,10 @@ void PhysicsServer3D::_bind_methods() {
 	BIND_ENUM_CONSTANT(AREA_PARAM_LINEAR_DAMP);
 	BIND_ENUM_CONSTANT(AREA_PARAM_ANGULAR_DAMP);
 	BIND_ENUM_CONSTANT(AREA_PARAM_PRIORITY);
+	BIND_ENUM_CONSTANT(AREA_PARAM_WIND_FORCE_MAGNITUDE);
+	BIND_ENUM_CONSTANT(AREA_PARAM_WIND_SOURCE);
+	BIND_ENUM_CONSTANT(AREA_PARAM_WIND_DIRECTION);
+	BIND_ENUM_CONSTANT(AREA_PARAM_WIND_ATTENUATION_FACTOR);
 
 	BIND_ENUM_CONSTANT(AREA_SPACE_OVERRIDE_DISABLED);
 	BIND_ENUM_CONSTANT(AREA_SPACE_OVERRIDE_COMBINE);
diff --git a/servers/physics_server_3d.h b/servers/physics_server_3d.h
index f806fd55be..5201e76498 100644
--- a/servers/physics_server_3d.h
+++ b/servers/physics_server_3d.h
@@ -298,7 +298,11 @@ public:
 		AREA_PARAM_GRAVITY_POINT_ATTENUATION,
 		AREA_PARAM_LINEAR_DAMP,
 		AREA_PARAM_ANGULAR_DAMP,
-		AREA_PARAM_PRIORITY
+		AREA_PARAM_PRIORITY,
+		AREA_PARAM_WIND_FORCE_MAGNITUDE,
+		AREA_PARAM_WIND_SOURCE,
+		AREA_PARAM_WIND_DIRECTION,
+		AREA_PARAM_WIND_ATTENUATION_FACTOR,
 	};
 
 	virtual RID area_create() = 0;