4 files changed, 30 insertions, 21 deletions

diff --git a/servers/physics_2d/area_2d_sw.cpp b/servers/physics_2d/area_2d_sw.cpp
index 532cb259b3..663a47f273 100644
--- a/servers/physics_2d/area_2d_sw.cpp
+++ b/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),
diff --git a/servers/physics_2d/area_2d_sw.h b/servers/physics_2d/area_2d_sw.h
index 3bf603b30d..d9147d6f1d 100644
--- a/servers/physics_2d/area_2d_sw.h
+++ b/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();
 };
diff --git a/servers/physics_2d/body_2d_sw.cpp b/servers/physics_2d/body_2d_sw.cpp
index 7aa2f9b7de..ad47912b82 100644
--- a/servers/physics_2d/body_2d_sw.cpp
+++ b/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;
 
diff --git a/servers/physics_2d/body_2d_sw.h b/servers/physics_2d/body_2d_sw.h
index 74bef433dc..30e14eb9c7 100644
--- a/servers/physics_2d/body_2d_sw.h
+++ b/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