Fix point gravity calculation

Removing the + 1 in point gravity formula when using distance scale to
make it more accurate for standard gravitation.

Fixes precession in orbits for games using gravitation.

Also moved gravity calculation to area to use it for both rigid bodies
and soft bodies in 3D (same change in 2D for consistency).

Co-authored-by: Ryan Peach <ryan.peach@keysight.com>

9 files changed, 63 insertions, 53 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
 
diff --git a/servers/physics_3d/area_3d_sw.cpp b/servers/physics_3d/area_3d_sw.cpp
index c292abad48..c9e8bcb8ca 100644
--- a/servers/physics_3d/area_3d_sw.cpp
+++ b/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),
diff --git a/servers/physics_3d/area_3d_sw.h b/servers/physics_3d/area_3d_sw.h
index 814472885c..d5f1e60119 100644
--- a/servers/physics_3d/area_3d_sw.h
+++ b/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();
 };
diff --git a/servers/physics_3d/body_3d_sw.cpp b/servers/physics_3d/body_3d_sw.cpp
index 0c4079332d..397a38079b 100644
--- a/servers/physics_3d/body_3d_sw.cpp
+++ b/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;
diff --git a/servers/physics_3d/body_3d_sw.h b/servers/physics_3d/body_3d_sw.h
index efb114a325..130be2d42f 100644
--- a/servers/physics_3d/body_3d_sw.h
+++ b/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();
 
diff --git a/servers/physics_3d/soft_body_3d_sw.cpp b/servers/physics_3d/soft_body_3d_sw.cpp
index ead6497f1f..d7e13867bf 100644
--- a/servers/physics_3d/soft_body_3d_sw.cpp
+++ b/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) {