Improve RigidDynamicBody force and torque API

Makes the API for forces and impulses more flexible, easier to
understand and harmonized between 2D and 3D.

Rigid bodies now have 3 sets of methods for forces and impulses:
-apply_impulse() for impulses (one-shot and time independent)
-apply_force() for forces (time dependent) applied for the current step
-add_constant_force() for forces that keeps being applied each step

Also updated the documentation to clarify the different methods and
parameters in rigid body nodes, body direct state and physics servers.

6 files changed, 194 insertions, 95 deletions

diff --git a/servers/physics_2d/godot_body_2d.cpp b/servers/physics_2d/godot_body_2d.cpp
index 44da5d4f3b..886fe4af0f 100644
--- a/servers/physics_2d/godot_body_2d.cpp
+++ b/servers/physics_2d/godot_body_2d.cpp
@@ -569,9 +569,8 @@ void GodotBody2D::integrate_forces(real_t p_step) {
 		if (!omit_force_integration) {
 			//overridden by direct state query
 
-			Vector2 force = gravity * mass;
-			force += applied_force;
-			real_t torque = applied_torque;
+			Vector2 force = gravity * mass + applied_force + constant_force;
+			real_t torque = applied_torque + constant_torque;
 
 			real_t damp = 1.0 - p_step * total_linear_damp;
 
@@ -598,7 +597,10 @@ void GodotBody2D::integrate_forces(real_t p_step) {
 		}
 	}
 
-	biased_angular_velocity = 0;
+	applied_force = Vector2();
+	applied_torque = 0.0;
+
+	biased_angular_velocity = 0.0;
 	biased_linear_velocity = Vector2();
 
 	if (do_motion) { //shapes temporarily extend for raycast
diff --git a/servers/physics_2d/godot_body_2d.h b/servers/physics_2d/godot_body_2d.h
index f00512fe92..9902da9b46 100644
--- a/servers/physics_2d/godot_body_2d.h
+++ b/servers/physics_2d/godot_body_2d.h
@@ -89,6 +89,9 @@ class GodotBody2D : public GodotCollisionObject2D {
 	Vector2 applied_force;
 	real_t applied_torque = 0.0;
 
+	Vector2 constant_force;
+	real_t constant_torque = 0.0;
+
 	SelfList<GodotBody2D> active_list;
 	SelfList<GodotBody2D> mass_properties_update_list;
 	SelfList<GodotBody2D> direct_state_query_list;
@@ -245,6 +248,38 @@ public:
 		}
 	}
 
+	_FORCE_INLINE_ void apply_central_force(const Vector2 &p_force) {
+		applied_force += p_force;
+	}
+
+	_FORCE_INLINE_ void apply_force(const Vector2 &p_force, const Vector2 &p_position = Vector2()) {
+		applied_force += p_force;
+		applied_torque += (p_position - center_of_mass).cross(p_force);
+	}
+
+	_FORCE_INLINE_ void apply_torque(real_t p_torque) {
+		applied_torque += p_torque;
+	}
+
+	_FORCE_INLINE_ void add_constant_central_force(const Vector2 &p_force) {
+		constant_force += p_force;
+	}
+
+	_FORCE_INLINE_ void add_constant_force(const Vector2 &p_force, const Vector2 &p_position = Vector2()) {
+		constant_force += p_force;
+		constant_torque += (p_position - center_of_mass).cross(p_force);
+	}
+
+	_FORCE_INLINE_ void add_constant_torque(real_t p_torque) {
+		constant_torque += p_torque;
+	}
+
+	void set_constant_force(const Vector2 &p_force) { constant_force = p_force; }
+	Vector2 get_constant_force() const { return constant_force; }
+
+	void set_constant_torque(real_t p_torque) { constant_torque = p_torque; }
+	real_t get_constant_torque() const { return constant_torque; }
+
 	void set_active(bool p_active);
 	_FORCE_INLINE_ bool is_active() const { return active; }
 
@@ -264,25 +299,6 @@ public:
 	void set_state(PhysicsServer2D::BodyState p_state, const Variant &p_variant);
 	Variant get_state(PhysicsServer2D::BodyState p_state) const;
 
-	void set_applied_force(const Vector2 &p_force) { applied_force = p_force; }
-	Vector2 get_applied_force() const { return applied_force; }
-
-	void set_applied_torque(real_t p_torque) { applied_torque = p_torque; }
-	real_t get_applied_torque() const { return applied_torque; }
-
-	_FORCE_INLINE_ void add_central_force(const Vector2 &p_force) {
-		applied_force += p_force;
-	}
-
-	_FORCE_INLINE_ void add_force(const Vector2 &p_force, const Vector2 &p_position = Vector2()) {
-		applied_force += p_force;
-		applied_torque += (p_position - center_of_mass).cross(p_force);
-	}
-
-	_FORCE_INLINE_ void add_torque(real_t p_torque) {
-		applied_torque += p_torque;
-	}
-
 	_FORCE_INLINE_ void set_continuous_collision_detection_mode(PhysicsServer2D::CCDMode p_mode) { continuous_cd_mode = p_mode; }
 	_FORCE_INLINE_ PhysicsServer2D::CCDMode get_continuous_collision_detection_mode() const { return continuous_cd_mode; }
 
diff --git a/servers/physics_2d/godot_body_direct_state_2d.cpp b/servers/physics_2d/godot_body_direct_state_2d.cpp
index 9c9bd56268..2843466e01 100644
--- a/servers/physics_2d/godot_body_direct_state_2d.cpp
+++ b/servers/physics_2d/godot_body_direct_state_2d.cpp
@@ -92,34 +92,71 @@ Vector2 GodotPhysicsDirectBodyState2D::get_velocity_at_local_position(const Vect
 	return body->get_velocity_in_local_point(p_position);
 }
 
-void GodotPhysicsDirectBodyState2D::add_central_force(const Vector2 &p_force) {
+void GodotPhysicsDirectBodyState2D::apply_central_impulse(const Vector2 &p_impulse) {
 	body->wakeup();
-	body->add_central_force(p_force);
+	body->apply_central_impulse(p_impulse);
 }
 
-void GodotPhysicsDirectBodyState2D::add_force(const Vector2 &p_force, const Vector2 &p_position) {
+void GodotPhysicsDirectBodyState2D::apply_impulse(const Vector2 &p_impulse, const Vector2 &p_position) {
 	body->wakeup();
-	body->add_force(p_force, p_position);
+	body->apply_impulse(p_impulse, p_position);
 }
 
-void GodotPhysicsDirectBodyState2D::add_torque(real_t p_torque) {
+void GodotPhysicsDirectBodyState2D::apply_torque_impulse(real_t p_torque) {
 	body->wakeup();
-	body->add_torque(p_torque);
+	body->apply_torque_impulse(p_torque);
 }
 
-void GodotPhysicsDirectBodyState2D::apply_central_impulse(const Vector2 &p_impulse) {
+void GodotPhysicsDirectBodyState2D::apply_central_force(const Vector2 &p_force) {
 	body->wakeup();
-	body->apply_central_impulse(p_impulse);
+	body->apply_central_force(p_force);
 }
 
-void GodotPhysicsDirectBodyState2D::apply_impulse(const Vector2 &p_impulse, const Vector2 &p_position) {
+void GodotPhysicsDirectBodyState2D::apply_force(const Vector2 &p_force, const Vector2 &p_position) {
 	body->wakeup();
-	body->apply_impulse(p_impulse, p_position);
+	body->apply_force(p_force, p_position);
 }
 
-void GodotPhysicsDirectBodyState2D::apply_torque_impulse(real_t p_torque) {
+void GodotPhysicsDirectBodyState2D::apply_torque(real_t p_torque) {
 	body->wakeup();
-	body->apply_torque_impulse(p_torque);
+	body->apply_torque(p_torque);
+}
+
+void GodotPhysicsDirectBodyState2D::add_constant_central_force(const Vector2 &p_force) {
+	body->wakeup();
+	body->add_constant_central_force(p_force);
+}
+
+void GodotPhysicsDirectBodyState2D::add_constant_force(const Vector2 &p_force, const Vector2 &p_position) {
+	body->wakeup();
+	body->add_constant_force(p_force, p_position);
+}
+
+void GodotPhysicsDirectBodyState2D::add_constant_torque(real_t p_torque) {
+	body->wakeup();
+	body->add_constant_torque(p_torque);
+}
+
+void GodotPhysicsDirectBodyState2D::set_constant_force(const Vector2 &p_force) {
+	if (!p_force.is_equal_approx(Vector2())) {
+		body->wakeup();
+	}
+	body->set_constant_force(p_force);
+}
+
+Vector2 GodotPhysicsDirectBodyState2D::get_constant_force() const {
+	return body->get_constant_force();
+}
+
+void GodotPhysicsDirectBodyState2D::set_constant_torque(real_t p_torque) {
+	if (!Math::is_zero_approx(p_torque)) {
+		body->wakeup();
+	}
+	body->set_constant_torque(p_torque);
+}
+
+real_t GodotPhysicsDirectBodyState2D::get_constant_torque() const {
+	return body->get_constant_torque();
 }
 
 void GodotPhysicsDirectBodyState2D::set_sleep_state(bool p_enable) {
diff --git a/servers/physics_2d/godot_body_direct_state_2d.h b/servers/physics_2d/godot_body_direct_state_2d.h
index ff25205d52..bc6038f7e8 100644
--- a/servers/physics_2d/godot_body_direct_state_2d.h
+++ b/servers/physics_2d/godot_body_direct_state_2d.h
@@ -61,13 +61,24 @@ public:
 
 	virtual Vector2 get_velocity_at_local_position(const Vector2 &p_position) const override;
 
-	virtual void add_central_force(const Vector2 &p_force) override;
-	virtual void add_force(const Vector2 &p_force, const Vector2 &p_position = Vector2()) override;
-	virtual void add_torque(real_t p_torque) override;
 	virtual void apply_central_impulse(const Vector2 &p_impulse) override;
 	virtual void apply_impulse(const Vector2 &p_impulse, const Vector2 &p_position = Vector2()) override;
 	virtual void apply_torque_impulse(real_t p_torque) override;
 
+	virtual void apply_central_force(const Vector2 &p_force) override;
+	virtual void apply_force(const Vector2 &p_force, const Vector2 &p_position = Vector2()) override;
+	virtual void apply_torque(real_t p_torque) override;
+
+	virtual void add_constant_central_force(const Vector2 &p_force) override;
+	virtual void add_constant_force(const Vector2 &p_force, const Vector2 &p_position = Vector2()) override;
+	virtual void add_constant_torque(real_t p_torque) override;
+
+	virtual void set_constant_force(const Vector2 &p_force) override;
+	virtual Vector2 get_constant_force() const override;
+
+	virtual void set_constant_torque(real_t p_torque) override;
+	virtual real_t get_constant_torque() const override;
+
 	virtual void set_sleep_state(bool p_enable) override;
 	virtual bool is_sleeping() const override;
 
diff --git a/servers/physics_2d/godot_physics_server_2d.cpp b/servers/physics_2d/godot_physics_server_2d.cpp
index c88fd39657..a1b2d8c3e0 100644
--- a/servers/physics_2d/godot_physics_server_2d.cpp
+++ b/servers/physics_2d/godot_physics_server_2d.cpp
@@ -669,68 +669,68 @@ void GodotPhysicsServer2D::body_attach_object_instance_id(RID p_body, ObjectID p
 	ERR_FAIL_COND(!body);
 
 	body->set_instance_id(p_id);
-};
+}
 
 ObjectID GodotPhysicsServer2D::body_get_object_instance_id(RID p_body) const {
 	GodotBody2D *body = body_owner.get_or_null(p_body);
 	ERR_FAIL_COND_V(!body, ObjectID());
 
 	return body->get_instance_id();
-};
+}
 
 void GodotPhysicsServer2D::body_attach_canvas_instance_id(RID p_body, ObjectID p_id) {
 	GodotBody2D *body = body_owner.get_or_null(p_body);
 	ERR_FAIL_COND(!body);
 
 	body->set_canvas_instance_id(p_id);
-};
+}
 
 ObjectID GodotPhysicsServer2D::body_get_canvas_instance_id(RID p_body) const {
 	GodotBody2D *body = body_owner.get_or_null(p_body);
 	ERR_FAIL_COND_V(!body, ObjectID());
 
 	return body->get_canvas_instance_id();
-};
+}
 
 void GodotPhysicsServer2D::body_set_collision_layer(RID p_body, uint32_t p_layer) {
 	GodotBody2D *body = body_owner.get_or_null(p_body);
 	ERR_FAIL_COND(!body);
 	body->set_collision_layer(p_layer);
-};
+}
 
 uint32_t GodotPhysicsServer2D::body_get_collision_layer(RID p_body) const {
 	GodotBody2D *body = body_owner.get_or_null(p_body);
 	ERR_FAIL_COND_V(!body, 0);
 
 	return body->get_collision_layer();
-};
+}
 
 void GodotPhysicsServer2D::body_set_collision_mask(RID p_body, uint32_t p_mask) {
 	GodotBody2D *body = body_owner.get_or_null(p_body);
 	ERR_FAIL_COND(!body);
 	body->set_collision_mask(p_mask);
-};
+}
 
 uint32_t GodotPhysicsServer2D::body_get_collision_mask(RID p_body) const {
 	GodotBody2D *body = body_owner.get_or_null(p_body);
 	ERR_FAIL_COND_V(!body, 0);
 
 	return body->get_collision_mask();
-};
+}
 
 void GodotPhysicsServer2D::body_set_param(RID p_body, BodyParameter p_param, const Variant &p_value) {
 	GodotBody2D *body = body_owner.get_or_null(p_body);
 	ERR_FAIL_COND(!body);
 
 	body->set_param(p_param, p_value);
-};
+}
 
 Variant GodotPhysicsServer2D::body_get_param(RID p_body, BodyParameter p_param) const {
 	GodotBody2D *body = body_owner.get_or_null(p_body);
 	ERR_FAIL_COND_V(!body, 0);
 
 	return body->get_param(p_param);
-};
+}
 
 void GodotPhysicsServer2D::body_reset_mass_properties(RID p_body) {
 	GodotBody2D *body = body_owner.get_or_null(p_body);
@@ -744,95 +744,123 @@ void GodotPhysicsServer2D::body_set_state(RID p_body, BodyState p_state, const V
 	ERR_FAIL_COND(!body);
 
 	body->set_state(p_state, p_variant);
-};
+}
 
 Variant GodotPhysicsServer2D::body_get_state(RID p_body, BodyState p_state) const {
 	GodotBody2D *body = body_owner.get_or_null(p_body);
 	ERR_FAIL_COND_V(!body, Variant());
 
 	return body->get_state(p_state);
-};
+}
 
-void GodotPhysicsServer2D::body_set_applied_force(RID p_body, const Vector2 &p_force) {
+void GodotPhysicsServer2D::body_apply_central_impulse(RID p_body, const Vector2 &p_impulse) {
 	GodotBody2D *body = body_owner.get_or_null(p_body);
 	ERR_FAIL_COND(!body);
 
-	body->set_applied_force(p_force);
+	body->apply_central_impulse(p_impulse);
 	body->wakeup();
-};
+}
 
-Vector2 GodotPhysicsServer2D::body_get_applied_force(RID p_body) const {
+void GodotPhysicsServer2D::body_apply_torque_impulse(RID p_body, real_t p_torque) {
 	GodotBody2D *body = body_owner.get_or_null(p_body);
-	ERR_FAIL_COND_V(!body, Vector2());
-	return body->get_applied_force();
-};
+	ERR_FAIL_COND(!body);
 
-void GodotPhysicsServer2D::body_set_applied_torque(RID p_body, real_t p_torque) {
+	_update_shapes();
+
+	body->apply_torque_impulse(p_torque);
+	body->wakeup();
+}
+
+void GodotPhysicsServer2D::body_apply_impulse(RID p_body, const Vector2 &p_impulse, const Vector2 &p_position) {
 	GodotBody2D *body = body_owner.get_or_null(p_body);
 	ERR_FAIL_COND(!body);
 
-	body->set_applied_torque(p_torque);
+	_update_shapes();
+
+	body->apply_impulse(p_impulse, p_position);
 	body->wakeup();
-};
+}
 
-real_t GodotPhysicsServer2D::body_get_applied_torque(RID p_body) const {
+void GodotPhysicsServer2D::body_apply_central_force(RID p_body, const Vector2 &p_force) {
 	GodotBody2D *body = body_owner.get_or_null(p_body);
-	ERR_FAIL_COND_V(!body, 0);
+	ERR_FAIL_COND(!body);
 
-	return body->get_applied_torque();
-};
+	body->apply_central_force(p_force);
+	body->wakeup();
+}
 
-void GodotPhysicsServer2D::body_apply_central_impulse(RID p_body, const Vector2 &p_impulse) {
+void GodotPhysicsServer2D::body_apply_force(RID p_body, const Vector2 &p_force, const Vector2 &p_position) {
 	GodotBody2D *body = body_owner.get_or_null(p_body);
 	ERR_FAIL_COND(!body);
 
-	body->apply_central_impulse(p_impulse);
+	body->apply_force(p_force, p_position);
 	body->wakeup();
 }
 
-void GodotPhysicsServer2D::body_apply_torque_impulse(RID p_body, real_t p_torque) {
+void GodotPhysicsServer2D::body_apply_torque(RID p_body, real_t p_torque) {
 	GodotBody2D *body = body_owner.get_or_null(p_body);
 	ERR_FAIL_COND(!body);
 
-	_update_shapes();
-
-	body->apply_torque_impulse(p_torque);
+	body->apply_torque(p_torque);
 	body->wakeup();
 }
 
-void GodotPhysicsServer2D::body_apply_impulse(RID p_body, const Vector2 &p_impulse, const Vector2 &p_position) {
+void GodotPhysicsServer2D::body_add_constant_central_force(RID p_body, const Vector2 &p_force) {
 	GodotBody2D *body = body_owner.get_or_null(p_body);
 	ERR_FAIL_COND(!body);
 
-	_update_shapes();
-
-	body->apply_impulse(p_impulse, p_position);
+	body->add_constant_central_force(p_force);
 	body->wakeup();
-};
+}
 
-void GodotPhysicsServer2D::body_add_central_force(RID p_body, const Vector2 &p_force) {
+void GodotPhysicsServer2D::body_add_constant_force(RID p_body, const Vector2 &p_force, const Vector2 &p_position) {
 	GodotBody2D *body = body_owner.get_or_null(p_body);
 	ERR_FAIL_COND(!body);
 
-	body->add_central_force(p_force);
+	body->add_constant_force(p_force, p_position);
 	body->wakeup();
-};
+}
 
-void GodotPhysicsServer2D::body_add_force(RID p_body, const Vector2 &p_force, const Vector2 &p_position) {
+void GodotPhysicsServer2D::body_add_constant_torque(RID p_body, real_t p_torque) {
 	GodotBody2D *body = body_owner.get_or_null(p_body);
 	ERR_FAIL_COND(!body);
 
-	body->add_force(p_force, p_position);
+	body->add_constant_torque(p_torque);
 	body->wakeup();
-};
+}
 
-void GodotPhysicsServer2D::body_add_torque(RID p_body, real_t p_torque) {
+void GodotPhysicsServer2D::body_set_constant_force(RID p_body, const Vector2 &p_force) {
 	GodotBody2D *body = body_owner.get_or_null(p_body);
 	ERR_FAIL_COND(!body);
 
-	body->add_torque(p_torque);
-	body->wakeup();
-};
+	body->set_constant_force(p_force);
+	if (!p_force.is_equal_approx(Vector2())) {
+		body->wakeup();
+	}
+}
+
+Vector2 GodotPhysicsServer2D::body_get_constant_force(RID p_body) const {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND_V(!body, Vector2());
+	return body->get_constant_force();
+}
+
+void GodotPhysicsServer2D::body_set_constant_torque(RID p_body, real_t p_torque) {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_constant_torque(p_torque);
+	if (!Math::is_zero_approx(p_torque)) {
+		body->wakeup();
+	}
+}
+
+real_t GodotPhysicsServer2D::body_get_constant_torque(RID p_body) const {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND_V(!body, 0);
+
+	return body->get_constant_torque();
+}
 
 void GodotPhysicsServer2D::body_set_axis_velocity(RID p_body, const Vector2 &p_axis_velocity) {
 	GodotBody2D *body = body_owner.get_or_null(p_body);
diff --git a/servers/physics_2d/godot_physics_server_2d.h b/servers/physics_2d/godot_physics_server_2d.h
index ad6d5e0940..bdf614a16d 100644
--- a/servers/physics_2d/godot_physics_server_2d.h
+++ b/servers/physics_2d/godot_physics_server_2d.h
@@ -207,19 +207,24 @@ public:
 	virtual void body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) override;
 	virtual Variant body_get_state(RID p_body, BodyState p_state) const override;
 
-	virtual void body_set_applied_force(RID p_body, const Vector2 &p_force) override;
-	virtual Vector2 body_get_applied_force(RID p_body) const override;
-
-	virtual void body_set_applied_torque(RID p_body, real_t p_torque) override;
-	virtual real_t body_get_applied_torque(RID p_body) const override;
-
-	virtual void body_add_central_force(RID p_body, const Vector2 &p_force) override;
-	virtual void body_add_force(RID p_body, const Vector2 &p_force, const Vector2 &p_position = Vector2()) override;
-	virtual void body_add_torque(RID p_body, real_t p_torque) override;
-
 	virtual void body_apply_central_impulse(RID p_body, const Vector2 &p_impulse) override;
 	virtual void body_apply_torque_impulse(RID p_body, real_t p_torque) override;
 	virtual void body_apply_impulse(RID p_body, const Vector2 &p_impulse, const Vector2 &p_position = Vector2()) override;
+
+	virtual void body_apply_central_force(RID p_body, const Vector2 &p_force) override;
+	virtual void body_apply_force(RID p_body, const Vector2 &p_force, const Vector2 &p_position = Vector2()) override;
+	virtual void body_apply_torque(RID p_body, real_t p_torque) override;
+
+	virtual void body_add_constant_central_force(RID p_body, const Vector2 &p_force) override;
+	virtual void body_add_constant_force(RID p_body, const Vector2 &p_force, const Vector2 &p_position = Vector2()) override;
+	virtual void body_add_constant_torque(RID p_body, real_t p_torque) override;
+
+	virtual void body_set_constant_force(RID p_body, const Vector2 &p_force) override;
+	virtual Vector2 body_get_constant_force(RID p_body) const override;
+
+	virtual void body_set_constant_torque(RID p_body, real_t p_torque) override;
+	virtual real_t body_get_constant_torque(RID p_body) const override;
+
 	virtual void body_set_axis_velocity(RID p_body, const Vector2 &p_axis_velocity) override;
 
 	virtual void body_add_collision_exception(RID p_body, RID p_body_b) override;