Merge pull request #52070 from nekomatata/area-point-gravity-fix

Fix point gravity calculation

servers/physics_2d/area_2d_sw.cpp

@@ -274,6 +274,26 @@ void Area2DSW::call_queries() {
 	}
 }
 
+void Area2DSW::compute_gravity(const Vector2 &p_position, Vector2 &r_gravity) const {
+	if (is_gravity_point()) {
+		const real_t gravity_distance_scale = get_gravity_distance_scale();
+		Vector2 v = get_transform().xform(get_gravity_vector()) - p_position;
+		if (gravity_distance_scale > 0) {
+			const real_t v_length = v.length();
+			if (v_length > 0) {
+				const real_t v_scaled = v_length * gravity_distance_scale;
+				r_gravity = (v.normalized() * (get_gravity() / (v_scaled * v_scaled)));
+			} else {
+				r_gravity = Vector2();
+			}
+		} else {
+			r_gravity = v.normalized() * get_gravity();
+		}
+	} else {
+		r_gravity = get_gravity_vector() * get_gravity();
+	}
+}
+
 Area2DSW::Area2DSW() :
 		CollisionObject2DSW(TYPE_AREA),
 		monitor_query_list(this),

servers/physics_2d/area_2d_sw.h

@@ -34,7 +34,6 @@
 #include "collision_object_2d_sw.h"
 #include "core/templates/self_list.h"
 #include "servers/physics_server_2d.h"
-//#include "servers/physics_3d/query_sw.h"
 
 class Space2DSW;
 class Body2DSW;
@@ -94,17 +93,12 @@ class Area2DSW : public CollisionObject2DSW {
 	Map<BodyKey, BodyState> monitored_bodies;
 	Map<BodyKey, BodyState> monitored_areas;
 
-	//virtual void shape_changed_notify(Shape2DSW *p_shape);
-	//virtual void shape_deleted_notify(Shape2DSW *p_shape);
 	Set<Constraint2DSW *> constraints;
 
 	virtual void _shapes_changed();
 	void _queue_monitor_update();
 
 public:
-	//_FORCE_INLINE_ const Matrix32& get_inverse_transform() const { return inverse_transform; }
-	//_FORCE_INLINE_ SpaceSW* get_owner() { return owner; }
-
 	void set_monitor_callback(ObjectID p_id, const StringName &p_method);
 	_FORCE_INLINE_ bool has_monitor_callback() const { return monitor_callback_id.is_valid(); }
 
@@ -161,6 +155,8 @@ public:
 
 	void call_queries();
 
+	void compute_gravity(const Vector2 &p_position, Vector2 &r_gravity) const;
+
 	Area2DSW();
 	~Area2DSW();
 };

servers/physics_2d/body_2d_sw.cpp

@@ -352,17 +352,10 @@ void Body2DSW::set_space(Space2DSW *p_space) {
 	first_integration = false;
 }
 
-void Body2DSW::_compute_area_gravity_and_dampenings(const Area2DSW *p_area) {
-	if (p_area->is_gravity_point()) {
-		if (p_area->get_gravity_distance_scale() > 0) {
-			Vector2 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 Body2DSW::_compute_area_gravity_and_damping(const Area2DSW *p_area) {
+	Vector2 area_gravity;
+	p_area->compute_gravity(get_transform().get_origin(), area_gravity);
+	gravity += area_gravity;
 
 	area_linear_damp += p_area->get_linear_damp();
 	area_angular_damp += p_area->get_angular_damp();
@@ -391,7 +384,7 @@ void Body2DSW::integrate_forces(real_t p_step) {
 			switch (mode) {
 				case PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE:
 				case PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: {
-					_compute_area_gravity_and_dampenings(aa[i].area);
+					_compute_area_gravity_and_damping(aa[i].area);
 					stopped = mode == PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE;
 				} break;
 				case PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE:
@@ -399,7 +392,7 @@ void Body2DSW::integrate_forces(real_t p_step) {
 					gravity = Vector2(0, 0);
 					area_angular_damp = 0;
 					area_linear_damp = 0;
-					_compute_area_gravity_and_dampenings(aa[i].area);
+					_compute_area_gravity_and_damping(aa[i].area);
 					stopped = mode == PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE;
 				} break;
 				default: {
@@ -408,7 +401,7 @@ void Body2DSW::integrate_forces(real_t p_step) {
 		}
 	}
 	if (!stopped) {
-		_compute_area_gravity_and_dampenings(def_area);
+		_compute_area_gravity_and_damping(def_area);
 	}
 	gravity *= gravity_scale;
 

servers/physics_2d/body_2d_sw.h

@@ -125,7 +125,7 @@ class Body2DSW : public CollisionObject2DSW {
 
 	uint64_t island_step;
 
-	_FORCE_INLINE_ void _compute_area_gravity_and_dampenings(const Area2DSW *p_area);
+	_FORCE_INLINE_ void _compute_area_gravity_and_damping(const Area2DSW *p_area);
 
 	friend class PhysicsDirectBodyState2DSW; // i give up, too many functions to expose
 

servers/physics_3d/area_3d_sw.cpp

@@ -304,6 +304,26 @@ void Area3DSW::call_queries() {
 	}
 }
 
+void Area3DSW::compute_gravity(const Vector3 &p_position, Vector3 &r_gravity) const {
+	if (is_gravity_point()) {
+		const real_t gravity_distance_scale = get_gravity_distance_scale();
+		Vector3 v = get_transform().xform(get_gravity_vector()) - p_position;
+		if (gravity_distance_scale > 0) {
+			const real_t v_length = v.length();
+			if (v_length > 0) {
+				const real_t v_scaled = v_length * gravity_distance_scale;
+				r_gravity = (v.normalized() * (get_gravity() / (v_scaled * v_scaled)));
+			} else {
+				r_gravity = Vector3();
+			}
+		} else {
+			r_gravity = v.normalized() * get_gravity();
+		}
+	} else {
+		r_gravity = get_gravity_vector() * get_gravity();
+	}
+}
+
 Area3DSW::Area3DSW() :
 		CollisionObject3DSW(TYPE_AREA),
 		monitor_query_list(this),

servers/physics_3d/area_3d_sw.h

@@ -34,7 +34,6 @@
 #include "collision_object_3d_sw.h"
 #include "core/templates/self_list.h"
 #include "servers/physics_server_3d.h"
-//#include "servers/physics_3d/query_sw.h"
 
 class Space3DSW;
 class Body3DSW;
@@ -101,18 +100,12 @@ class Area3DSW : public CollisionObject3DSW {
 	Map<BodyKey, BodyState> monitored_bodies;
 	Map<BodyKey, BodyState> monitored_areas;
 
-	//virtual void shape_changed_notify(ShapeSW *p_shape);
-	//virtual void shape_deleted_notify(ShapeSW *p_shape);
-
 	Set<Constraint3DSW *> constraints;
 
 	virtual void _shapes_changed();
 	void _queue_monitor_update();
 
 public:
-	//_FORCE_INLINE_ const Transform& get_inverse_transform() const { return inverse_transform; }
-	//_FORCE_INLINE_ SpaceSW* get_owner() { return owner; }
-
 	void set_monitor_callback(ObjectID p_id, const StringName &p_method);
 	_FORCE_INLINE_ bool has_monitor_callback() const { return monitor_callback_id.is_valid(); }
 
@@ -184,6 +177,8 @@ public:
 
 	void call_queries();
 
+	void compute_gravity(const Vector3 &p_position, Vector3 &r_gravity) const;
+
 	Area3DSW();
 	~Area3DSW();
 };

servers/physics_3d/body_3d_sw.cpp

@@ -379,17 +379,10 @@ void Body3DSW::set_space(Space3DSW *p_space) {
 	first_integration = true;
 }
 
-void Body3DSW::_compute_area_gravity_and_dampenings(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 Body3DSW::_compute_area_gravity_and_damping(const Area3DSW *p_area) {
+	Vector3 area_gravity;
+	p_area->compute_gravity(get_transform().get_origin(), area_gravity);
+	gravity += area_gravity;
 
 	area_linear_damp += p_area->get_linear_damp();
 	area_angular_damp += p_area->get_angular_damp();
@@ -431,7 +424,7 @@ void Body3DSW::integrate_forces(real_t p_step) {
 			switch (mode) {
 				case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE:
 				case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: {
-					_compute_area_gravity_and_dampenings(aa[i].area);
+					_compute_area_gravity_and_damping(aa[i].area);
 					stopped = mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE;
 				} break;
 				case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE:
@@ -439,7 +432,7 @@ void Body3DSW::integrate_forces(real_t p_step) {
 					gravity = Vector3(0, 0, 0);
 					area_angular_damp = 0;
 					area_linear_damp = 0;
-					_compute_area_gravity_and_dampenings(aa[i].area);
+					_compute_area_gravity_and_damping(aa[i].area);
 					stopped = mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE;
 				} break;
 				default: {
@@ -449,7 +442,7 @@ void Body3DSW::integrate_forces(real_t p_step) {
 	}
 
 	if (!stopped) {
-		_compute_area_gravity_and_dampenings(def_area);
+		_compute_area_gravity_and_damping(def_area);
 	}
 
 	gravity *= gravity_scale;

servers/physics_3d/body_3d_sw.h

@@ -122,7 +122,7 @@ class Body3DSW : public CollisionObject3DSW {
 
 	uint64_t island_step;
 
-	_FORCE_INLINE_ void _compute_area_gravity_and_dampenings(const Area3DSW *p_area);
+	_FORCE_INLINE_ void _compute_area_gravity_and_damping(const Area3DSW *p_area);
 
 	_FORCE_INLINE_ void _update_transform_dependant();
 

servers/physics_3d/soft_body_3d_sw.cpp

@@ -964,16 +964,9 @@ void SoftBody3DSW::apply_forces(bool p_has_wind_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();
-	}
+	Vector3 area_gravity;
+	p_area->compute_gravity(get_transform().get_origin(), area_gravity);
+	gravity += area_gravity;
 }
 
 Vector3 SoftBody3DSW::_compute_area_windforce(const Area3DSW *p_area, const Face *p_face) {