Merge pull request #55736 from nekomatata/physics-apply-forces

Improve RigidDynamicBody force and torque API

6 files changed, 177 insertions, 81 deletions

diff --git a/servers/physics_3d/godot_body_3d.cpp b/servers/physics_3d/godot_body_3d.cpp
index d5098a2a5f..fca8a3f3a9 100644
--- a/servers/physics_3d/godot_body_3d.cpp
+++ b/servers/physics_3d/godot_body_3d.cpp
@@ -628,9 +628,8 @@ void GodotBody3D::integrate_forces(real_t p_step) {
 		if (!omit_force_integration) {
 			//overridden by direct state query
 
-			Vector3 force = gravity * mass;
-			force += applied_force;
-			Vector3 torque = applied_torque;
+			Vector3 force = gravity * mass + applied_force + constant_force;
+			Vector3 torque = applied_torque + constant_torque;
 
 			real_t damp = 1.0 - p_step * total_linear_damp;
 
diff --git a/servers/physics_3d/godot_body_3d.h b/servers/physics_3d/godot_body_3d.h
index 6ea6d1fcaa..5868d8c282 100644
--- a/servers/physics_3d/godot_body_3d.h
+++ b/servers/physics_3d/godot_body_3d.h
@@ -93,6 +93,9 @@ class GodotBody3D : public GodotCollisionObject3D {
 	Vector3 applied_force;
 	Vector3 applied_torque;
 
+	Vector3 constant_force;
+	Vector3 constant_torque;
+
 	SelfList<GodotBody3D> active_list;
 	SelfList<GodotBody3D> mass_properties_update_list;
 	SelfList<GodotBody3D> direct_state_query_list;
@@ -244,19 +247,38 @@ public:
 		biased_angular_velocity += _inv_inertia_tensor.xform(p_impulse);
 	}
 
-	_FORCE_INLINE_ void add_central_force(const Vector3 &p_force) {
+	_FORCE_INLINE_ void apply_central_force(const Vector3 &p_force) {
 		applied_force += p_force;
 	}
 
-	_FORCE_INLINE_ void add_force(const Vector3 &p_force, const Vector3 &p_position = Vector3()) {
+	_FORCE_INLINE_ void apply_force(const Vector3 &p_force, const Vector3 &p_position = Vector3()) {
 		applied_force += p_force;
 		applied_torque += (p_position - center_of_mass).cross(p_force);
 	}
 
-	_FORCE_INLINE_ void add_torque(const Vector3 &p_torque) {
+	_FORCE_INLINE_ void apply_torque(const Vector3 &p_torque) {
 		applied_torque += p_torque;
 	}
 
+	_FORCE_INLINE_ void add_constant_central_force(const Vector3 &p_force) {
+		constant_force += p_force;
+	}
+
+	_FORCE_INLINE_ void add_constant_force(const Vector3 &p_force, const Vector3 &p_position = Vector3()) {
+		constant_force += p_force;
+		constant_torque += (p_position - center_of_mass).cross(p_force);
+	}
+
+	_FORCE_INLINE_ void add_constant_torque(const Vector3 &p_torque) {
+		constant_torque += p_torque;
+	}
+
+	void set_constant_force(const Vector3 &p_force) { constant_force = p_force; }
+	Vector3 get_constant_force() const { return constant_force; }
+
+	void set_constant_torque(const Vector3 &p_torque) { constant_torque = p_torque; }
+	Vector3 get_constant_torque() const { return constant_torque; }
+
 	void set_active(bool p_active);
 	_FORCE_INLINE_ bool is_active() const { return active; }
 
@@ -276,12 +298,6 @@ public:
 	void set_state(PhysicsServer3D::BodyState p_state, const Variant &p_variant);
 	Variant get_state(PhysicsServer3D::BodyState p_state) const;
 
-	void set_applied_force(const Vector3 &p_force) { applied_force = p_force; }
-	Vector3 get_applied_force() const { return applied_force; }
-
-	void set_applied_torque(const Vector3 &p_torque) { applied_torque = p_torque; }
-	Vector3 get_applied_torque() const { return applied_torque; }
-
 	_FORCE_INLINE_ void set_continuous_collision_detection(bool p_enable) { continuous_cd = p_enable; }
 	_FORCE_INLINE_ bool is_continuous_collision_detection_enabled() const { return continuous_cd; }
 
diff --git a/servers/physics_3d/godot_body_direct_state_3d.cpp b/servers/physics_3d/godot_body_direct_state_3d.cpp
index a929cab6f9..e45180cd02 100644
--- a/servers/physics_3d/godot_body_direct_state_3d.cpp
+++ b/servers/physics_3d/godot_body_direct_state_3d.cpp
@@ -99,34 +99,71 @@ Vector3 GodotPhysicsDirectBodyState3D::get_velocity_at_local_position(const Vect
 	return body->get_velocity_in_local_point(p_position);
 }
 
-void GodotPhysicsDirectBodyState3D::add_central_force(const Vector3 &p_force) {
+void GodotPhysicsDirectBodyState3D::apply_central_impulse(const Vector3 &p_impulse) {
 	body->wakeup();
-	body->add_central_force(p_force);
+	body->apply_central_impulse(p_impulse);
 }
 
-void GodotPhysicsDirectBodyState3D::add_force(const Vector3 &p_force, const Vector3 &p_position) {
+void GodotPhysicsDirectBodyState3D::apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position) {
 	body->wakeup();
-	body->add_force(p_force, p_position);
+	body->apply_impulse(p_impulse, p_position);
 }
 
-void GodotPhysicsDirectBodyState3D::add_torque(const Vector3 &p_torque) {
+void GodotPhysicsDirectBodyState3D::apply_torque_impulse(const Vector3 &p_impulse) {
 	body->wakeup();
-	body->add_torque(p_torque);
+	body->apply_torque_impulse(p_impulse);
 }
 
-void GodotPhysicsDirectBodyState3D::apply_central_impulse(const Vector3 &p_impulse) {
+void GodotPhysicsDirectBodyState3D::apply_central_force(const Vector3 &p_force) {
 	body->wakeup();
-	body->apply_central_impulse(p_impulse);
+	body->apply_central_force(p_force);
 }
 
-void GodotPhysicsDirectBodyState3D::apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position) {
+void GodotPhysicsDirectBodyState3D::apply_force(const Vector3 &p_force, const Vector3 &p_position) {
 	body->wakeup();
-	body->apply_impulse(p_impulse, p_position);
+	body->apply_force(p_force, p_position);
 }
 
-void GodotPhysicsDirectBodyState3D::apply_torque_impulse(const Vector3 &p_impulse) {
+void GodotPhysicsDirectBodyState3D::apply_torque(const Vector3 &p_torque) {
 	body->wakeup();
-	body->apply_torque_impulse(p_impulse);
+	body->apply_torque(p_torque);
+}
+
+void GodotPhysicsDirectBodyState3D::add_constant_central_force(const Vector3 &p_force) {
+	body->wakeup();
+	body->add_constant_central_force(p_force);
+}
+
+void GodotPhysicsDirectBodyState3D::add_constant_force(const Vector3 &p_force, const Vector3 &p_position) {
+	body->wakeup();
+	body->add_constant_force(p_force, p_position);
+}
+
+void GodotPhysicsDirectBodyState3D::add_constant_torque(const Vector3 &p_torque) {
+	body->wakeup();
+	body->add_constant_torque(p_torque);
+}
+
+void GodotPhysicsDirectBodyState3D::set_constant_force(const Vector3 &p_force) {
+	if (!p_force.is_equal_approx(Vector3())) {
+		body->wakeup();
+	}
+	body->set_constant_force(p_force);
+}
+
+Vector3 GodotPhysicsDirectBodyState3D::get_constant_force() const {
+	return body->get_constant_force();
+}
+
+void GodotPhysicsDirectBodyState3D::set_constant_torque(const Vector3 &p_torque) {
+	if (!p_torque.is_equal_approx(Vector3())) {
+		body->wakeup();
+	}
+	body->set_constant_torque(p_torque);
+}
+
+Vector3 GodotPhysicsDirectBodyState3D::get_constant_torque() const {
+	return body->get_constant_torque();
 }
 
 void GodotPhysicsDirectBodyState3D::set_sleep_state(bool p_sleep) {
diff --git a/servers/physics_3d/godot_body_direct_state_3d.h b/servers/physics_3d/godot_body_direct_state_3d.h
index 35fd1543b0..93e9997e11 100644
--- a/servers/physics_3d/godot_body_direct_state_3d.h
+++ b/servers/physics_3d/godot_body_direct_state_3d.h
@@ -64,13 +64,24 @@ public:
 
 	virtual Vector3 get_velocity_at_local_position(const Vector3 &p_position) const override;
 
-	virtual void add_central_force(const Vector3 &p_force) override;
-	virtual void add_force(const Vector3 &p_force, const Vector3 &p_position = Vector3()) override;
-	virtual void add_torque(const Vector3 &p_torque) override;
 	virtual void apply_central_impulse(const Vector3 &p_impulse) override;
 	virtual void apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position = Vector3()) override;
 	virtual void apply_torque_impulse(const Vector3 &p_impulse) override;
 
+	virtual void apply_central_force(const Vector3 &p_force) override;
+	virtual void apply_force(const Vector3 &p_force, const Vector3 &p_position = Vector3()) override;
+	virtual void apply_torque(const Vector3 &p_torque) override;
+
+	virtual void add_constant_central_force(const Vector3 &p_force) override;
+	virtual void add_constant_force(const Vector3 &p_force, const Vector3 &p_position = Vector3()) override;
+	virtual void add_constant_torque(const Vector3 &p_torque) override;
+
+	virtual void set_constant_force(const Vector3 &p_force) override;
+	virtual Vector3 get_constant_force() const override;
+
+	virtual void set_constant_torque(const Vector3 &p_torque) override;
+	virtual Vector3 get_constant_torque() const override;
+
 	virtual void set_sleep_state(bool p_sleep) override;
 	virtual bool is_sleeping() const override;
 
diff --git a/servers/physics_3d/godot_physics_server_3d.cpp b/servers/physics_3d/godot_physics_server_3d.cpp
index 573a5d373f..f301b791bb 100644
--- a/servers/physics_3d/godot_physics_server_3d.cpp
+++ b/servers/physics_3d/godot_physics_server_3d.cpp
@@ -607,40 +607,40 @@ void GodotPhysicsServer3D::body_attach_object_instance_id(RID p_body, ObjectID p
 	}
 
 	ERR_FAIL_MSG("Invalid ID.");
-};
+}
 
 ObjectID GodotPhysicsServer3D::body_get_object_instance_id(RID p_body) const {
 	GodotBody3D *body = body_owner.get_or_null(p_body);
 	ERR_FAIL_COND_V(!body, ObjectID());
 
 	return body->get_instance_id();
-};
+}
 
 void GodotPhysicsServer3D::body_set_user_flags(RID p_body, uint32_t p_flags) {
 	GodotBody3D *body = body_owner.get_or_null(p_body);
 	ERR_FAIL_COND(!body);
-};
+}
 
 uint32_t GodotPhysicsServer3D::body_get_user_flags(RID p_body) const {
 	GodotBody3D *body = body_owner.get_or_null(p_body);
 	ERR_FAIL_COND_V(!body, 0);
 
 	return 0;
-};
+}
 
 void GodotPhysicsServer3D::body_set_param(RID p_body, BodyParameter p_param, const Variant &p_value) {
 	GodotBody3D *body = body_owner.get_or_null(p_body);
 	ERR_FAIL_COND(!body);
 
 	body->set_param(p_param, p_value);
-};
+}
 
 Variant GodotPhysicsServer3D::body_get_param(RID p_body, BodyParameter p_param) const {
 	GodotBody3D *body = body_owner.get_or_null(p_body);
 	ERR_FAIL_COND_V(!body, 0);
 
 	return body->get_param(p_param);
-};
+}
 
 void GodotPhysicsServer3D::body_reset_mass_properties(RID p_body) {
 	GodotBody3D *body = body_owner.get_or_null(p_body);
@@ -654,97 +654,125 @@ void GodotPhysicsServer3D::body_set_state(RID p_body, BodyState p_state, const V
 	ERR_FAIL_COND(!body);
 
 	body->set_state(p_state, p_variant);
-};
+}
 
 Variant GodotPhysicsServer3D::body_get_state(RID p_body, BodyState p_state) const {
 	GodotBody3D *body = body_owner.get_or_null(p_body);
 	ERR_FAIL_COND_V(!body, Variant());
 
 	return body->get_state(p_state);
-};
+}
 
-void GodotPhysicsServer3D::body_set_applied_force(RID p_body, const Vector3 &p_force) {
+void GodotPhysicsServer3D::body_apply_central_impulse(RID p_body, const Vector3 &p_impulse) {
 	GodotBody3D *body = body_owner.get_or_null(p_body);
 	ERR_FAIL_COND(!body);
 
-	body->set_applied_force(p_force);
+	_update_shapes();
+
+	body->apply_central_impulse(p_impulse);
 	body->wakeup();
-};
+}
 
-Vector3 GodotPhysicsServer3D::body_get_applied_force(RID p_body) const {
+void GodotPhysicsServer3D::body_apply_impulse(RID p_body, const Vector3 &p_impulse, const Vector3 &p_position) {
 	GodotBody3D *body = body_owner.get_or_null(p_body);
-	ERR_FAIL_COND_V(!body, Vector3());
-	return body->get_applied_force();
-};
+	ERR_FAIL_COND(!body);
+
+	_update_shapes();
 
-void GodotPhysicsServer3D::body_set_applied_torque(RID p_body, const Vector3 &p_torque) {
+	body->apply_impulse(p_impulse, p_position);
+	body->wakeup();
+}
+
+void GodotPhysicsServer3D::body_apply_torque_impulse(RID p_body, const Vector3 &p_impulse) {
 	GodotBody3D *body = body_owner.get_or_null(p_body);
 	ERR_FAIL_COND(!body);
 
-	body->set_applied_torque(p_torque);
+	_update_shapes();
+
+	body->apply_torque_impulse(p_impulse);
 	body->wakeup();
-};
+}
 
-Vector3 GodotPhysicsServer3D::body_get_applied_torque(RID p_body) const {
+void GodotPhysicsServer3D::body_apply_central_force(RID p_body, const Vector3 &p_force) {
 	GodotBody3D *body = body_owner.get_or_null(p_body);
-	ERR_FAIL_COND_V(!body, Vector3());
+	ERR_FAIL_COND(!body);
 
-	return body->get_applied_torque();
-};
+	body->apply_central_force(p_force);
+	body->wakeup();
+}
 
-void GodotPhysicsServer3D::body_add_central_force(RID p_body, const Vector3 &p_force) {
+void GodotPhysicsServer3D::body_apply_force(RID p_body, const Vector3 &p_force, const Vector3 &p_position) {
 	GodotBody3D *body = body_owner.get_or_null(p_body);
 	ERR_FAIL_COND(!body);
 
-	body->add_central_force(p_force);
+	body->apply_force(p_force, p_position);
 	body->wakeup();
 }
 
-void GodotPhysicsServer3D::body_add_force(RID p_body, const Vector3 &p_force, const Vector3 &p_position) {
+void GodotPhysicsServer3D::body_apply_torque(RID p_body, const Vector3 &p_torque) {
 	GodotBody3D *body = body_owner.get_or_null(p_body);
 	ERR_FAIL_COND(!body);
 
-	body->add_force(p_force, p_position);
+	body->apply_torque(p_torque);
 	body->wakeup();
-};
+}
 
-void GodotPhysicsServer3D::body_add_torque(RID p_body, const Vector3 &p_torque) {
+void GodotPhysicsServer3D::body_add_constant_central_force(RID p_body, const Vector3 &p_force) {
 	GodotBody3D *body = body_owner.get_or_null(p_body);
 	ERR_FAIL_COND(!body);
 
-	body->add_torque(p_torque);
+	body->add_constant_central_force(p_force);
 	body->wakeup();
-};
+}
 
-void GodotPhysicsServer3D::body_apply_central_impulse(RID p_body, const Vector3 &p_impulse) {
+void GodotPhysicsServer3D::body_add_constant_force(RID p_body, const Vector3 &p_force, const Vector3 &p_position) {
 	GodotBody3D *body = body_owner.get_or_null(p_body);
 	ERR_FAIL_COND(!body);
 
-	_update_shapes();
+	body->add_constant_force(p_force, p_position);
+	body->wakeup();
+}
 
-	body->apply_central_impulse(p_impulse);
+void GodotPhysicsServer3D::body_add_constant_torque(RID p_body, const Vector3 &p_torque) {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->add_constant_torque(p_torque);
 	body->wakeup();
 }
 
-void GodotPhysicsServer3D::body_apply_impulse(RID p_body, const Vector3 &p_impulse, const Vector3 &p_position) {
+void GodotPhysicsServer3D::body_set_constant_force(RID p_body, const Vector3 &p_force) {
 	GodotBody3D *body = body_owner.get_or_null(p_body);
 	ERR_FAIL_COND(!body);
 
-	_update_shapes();
+	body->set_constant_force(p_force);
+	if (!p_force.is_equal_approx(Vector3())) {
+		body->wakeup();
+	}
+}
 
-	body->apply_impulse(p_impulse, p_position);
-	body->wakeup();
-};
+Vector3 GodotPhysicsServer3D::body_get_constant_force(RID p_body) const {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND_V(!body, Vector3());
+	return body->get_constant_force();
+}
 
-void GodotPhysicsServer3D::body_apply_torque_impulse(RID p_body, const Vector3 &p_impulse) {
+void GodotPhysicsServer3D::body_set_constant_torque(RID p_body, const Vector3 &p_torque) {
 	GodotBody3D *body = body_owner.get_or_null(p_body);
 	ERR_FAIL_COND(!body);
 
-	_update_shapes();
+	body->set_constant_torque(p_torque);
+	if (!p_torque.is_equal_approx(Vector3())) {
+		body->wakeup();
+	}
+}
 
-	body->apply_torque_impulse(p_impulse);
-	body->wakeup();
-};
+Vector3 GodotPhysicsServer3D::body_get_constant_torque(RID p_body) const {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND_V(!body, Vector3());
+
+	return body->get_constant_torque();
+}
 
 void GodotPhysicsServer3D::body_set_axis_velocity(RID p_body, const Vector3 &p_axis_velocity) {
 	GodotBody3D *body = body_owner.get_or_null(p_body);
@@ -758,7 +786,7 @@ void GodotPhysicsServer3D::body_set_axis_velocity(RID p_body, const Vector3 &p_a
 	v += p_axis_velocity;
 	body->set_linear_velocity(v);
 	body->wakeup();
-};
+}
 
 void GodotPhysicsServer3D::body_set_axis_lock(RID p_body, BodyAxis p_axis, bool p_lock) {
 	GodotBody3D *body = body_owner.get_or_null(p_body);
diff --git a/servers/physics_3d/godot_physics_server_3d.h b/servers/physics_3d/godot_physics_server_3d.h
index be9bbea76b..ab9cd19723 100644
--- a/servers/physics_3d/godot_physics_server_3d.h
+++ b/servers/physics_3d/godot_physics_server_3d.h
@@ -203,19 +203,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 Vector3 &p_force) override;
-	virtual Vector3 body_get_applied_force(RID p_body) const override;
-
-	virtual void body_set_applied_torque(RID p_body, const Vector3 &p_torque) override;
-	virtual Vector3 body_get_applied_torque(RID p_body) const override;
-
-	virtual void body_add_central_force(RID p_body, const Vector3 &p_force) override;
-	virtual void body_add_force(RID p_body, const Vector3 &p_force, const Vector3 &p_position = Vector3()) override;
-	virtual void body_add_torque(RID p_body, const Vector3 &p_torque) override;
-
 	virtual void body_apply_central_impulse(RID p_body, const Vector3 &p_impulse) override;
 	virtual void body_apply_impulse(RID p_body, const Vector3 &p_impulse, const Vector3 &p_position = Vector3()) override;
 	virtual void body_apply_torque_impulse(RID p_body, const Vector3 &p_impulse) override;
+
+	virtual void body_apply_central_force(RID p_body, const Vector3 &p_force) override;
+	virtual void body_apply_force(RID p_body, const Vector3 &p_force, const Vector3 &p_position = Vector3()) override;
+	virtual void body_apply_torque(RID p_body, const Vector3 &p_torque) override;
+
+	virtual void body_add_constant_central_force(RID p_body, const Vector3 &p_force) override;
+	virtual void body_add_constant_force(RID p_body, const Vector3 &p_force, const Vector3 &p_position = Vector3()) override;
+	virtual void body_add_constant_torque(RID p_body, const Vector3 &p_torque) override;
+
+	virtual void body_set_constant_force(RID p_body, const Vector3 &p_force) override;
+	virtual Vector3 body_get_constant_force(RID p_body) const override;
+
+	virtual void body_set_constant_torque(RID p_body, const Vector3 &p_torque) override;
+	virtual Vector3 body_get_constant_torque(RID p_body) const override;
+
 	virtual void body_set_axis_velocity(RID p_body, const Vector3 &p_axis_velocity) override;
 
 	virtual void body_set_axis_lock(RID p_body, BodyAxis p_axis, bool p_lock) override;