Merge pull request #20404 from TigerCaldwell/master

Ensured consistency between RigidBody, PhysicsDirectBodyState, PhysicsServers and their 2D counterparts

4 files changed, 64 insertions, 2 deletions

diff --git a/servers/physics_2d/body_2d_sw.h b/servers/physics_2d/body_2d_sw.h
index 301bd6b299..7fe805b1f9 100644
--- a/servers/physics_2d/body_2d_sw.h
+++ b/servers/physics_2d/body_2d_sw.h
@@ -201,12 +201,20 @@ public:
 	_FORCE_INLINE_ void set_biased_angular_velocity(real_t p_velocity) { biased_angular_velocity = p_velocity; }
 	_FORCE_INLINE_ real_t get_biased_angular_velocity() const { return biased_angular_velocity; }
 
+	_FORCE_INLINE_ void apply_central_impulse(const Vector2 &p_impulse) {
+		linear_velocity += p_impulse * _inv_mass;
+	}
+
 	_FORCE_INLINE_ void apply_impulse(const Vector2 &p_offset, const Vector2 &p_impulse) {
 
 		linear_velocity += p_impulse * _inv_mass;
 		angular_velocity += _inv_inertia * p_offset.cross(p_impulse);
 	}
 
+	_FORCE_INLINE_ void apply_torque_impulse(real_t p_torque) {
+		angular_velocity += _inv_inertia * p_torque;
+	}
+
 	_FORCE_INLINE_ void apply_bias_impulse(const Vector2 &p_pos, const Vector2 &p_j) {
 
 		biased_linear_velocity += p_j * _inv_mass;
@@ -237,12 +245,20 @@ public:
 	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_force(const Vector2 &p_force, const Vector2 &p_offset) {
+	_FORCE_INLINE_ void add_central_force(const Vector2 &p_force) {
+		applied_force += p_force;
+	}
+
+	_FORCE_INLINE_ void add_force(const Vector2 &p_offset, const Vector2 &p_force) {
 
 		applied_force += p_force;
 		applied_torque += p_offset.cross(p_force);
 	}
 
+	_FORCE_INLINE_ void add_torque(real_t p_torque) {
+		applied_torque += p_torque;
+	}
+
 	_FORCE_INLINE_ void set_continuous_collision_detection_mode(Physics2DServer::CCDMode p_mode) { continuous_cd_mode = p_mode; }
 	_FORCE_INLINE_ Physics2DServer::CCDMode get_continuous_collision_detection_mode() const { return continuous_cd_mode; }
 
@@ -357,6 +373,13 @@ public:
 	virtual void set_transform(const Transform2D &p_transform) { body->set_state(Physics2DServer::BODY_STATE_TRANSFORM, p_transform); }
 	virtual Transform2D get_transform() const { return body->get_transform(); }
 
+	virtual void add_central_force(const Vector2 &p_force) { body->add_central_force(p_force); }
+	virtual void add_force(const Vector2 &p_offset, const Vector2 &p_force) { body->add_force(p_offset, p_force); }
+	virtual void add_torque(real_t p_torque) { body->add_torque(p_torque); }
+	virtual void apply_central_impulse(const Vector2 &p_impulse) { body->apply_central_impulse(p_impulse); }
+	virtual void apply_impulse(const Vector2 &p_offset, const Vector2 &p_force) { body->apply_impulse(p_offset, p_force); }
+	virtual void apply_torque_impulse(real_t p_torque) { body->apply_torque_impulse(p_torque); }
+
 	virtual void set_sleep_state(bool p_enable) { body->set_active(!p_enable); }
 	virtual bool is_sleeping() const { return !body->is_active(); }
 
diff --git a/servers/physics_2d/physics_2d_server_sw.cpp b/servers/physics_2d/physics_2d_server_sw.cpp
index cfcef8cb04..ba87969eea 100644
--- a/servers/physics_2d/physics_2d_server_sw.cpp
+++ b/servers/physics_2d/physics_2d_server_sw.cpp
@@ -854,6 +854,21 @@ real_t Physics2DServerSW::body_get_applied_torque(RID p_body) const {
 	return body->get_applied_torque();
 };
 
+void Physics2DServerSW::body_apply_central_impulse(RID p_body, const Vector2 &p_impulse) {
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->apply_central_impulse(p_impulse);
+	body->wakeup();
+}
+
+void Physics2DServerSW::body_apply_torque_impulse(RID p_body, real_t p_torque) {
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->apply_torque_impulse(p_torque);
+}
+
 void Physics2DServerSW::body_apply_impulse(RID p_body, const Vector2 &p_pos, const Vector2 &p_impulse) {
 
 	Body2DSW *body = body_owner.get(p_body);
@@ -863,12 +878,28 @@ void Physics2DServerSW::body_apply_impulse(RID p_body, const Vector2 &p_pos, con
 	body->wakeup();
 };
 
+void Physics2DServerSW::body_add_central_force(RID p_body, const Vector2 &p_force) {
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->add_central_force(p_force);
+	body->wakeup();
+};
+
 void Physics2DServerSW::body_add_force(RID p_body, const Vector2 &p_offset, const Vector2 &p_force) {
 
 	Body2DSW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
 
-	body->add_force(p_force, p_offset);
+	body->add_force(p_offset, p_force);
+	body->wakeup();
+};
+
+void Physics2DServerSW::body_add_torque(RID p_body, real_t p_torque) {
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->add_torque(p_torque);
 	body->wakeup();
 };
 
diff --git a/servers/physics_2d/physics_2d_server_sw.h b/servers/physics_2d/physics_2d_server_sw.h
index bf00746063..0b8d3f2a31 100644
--- a/servers/physics_2d/physics_2d_server_sw.h
+++ b/servers/physics_2d/physics_2d_server_sw.h
@@ -213,8 +213,12 @@ public:
 	virtual void body_set_applied_torque(RID p_body, real_t p_torque);
 	virtual real_t body_get_applied_torque(RID p_body) const;
 
+	virtual void body_add_central_force(RID p_body, const Vector2 &p_force);
 	virtual void body_add_force(RID p_body, const Vector2 &p_offset, const Vector2 &p_force);
+	virtual void body_add_torque(RID p_body, real_t p_torque);
 
+	virtual void body_apply_central_impulse(RID p_body, const Vector2 &p_impulse);
+	virtual void body_apply_torque_impulse(RID p_body, real_t p_torque);
 	virtual void body_apply_impulse(RID p_body, const Vector2 &p_pos, const Vector2 &p_impulse);
 	virtual void body_set_axis_velocity(RID p_body, const Vector2 &p_axis_velocity);
 
diff --git a/servers/physics_2d/physics_2d_server_wrap_mt.h b/servers/physics_2d/physics_2d_server_wrap_mt.h
index a85cd5ef8d..b9b0f80805 100644
--- a/servers/physics_2d/physics_2d_server_wrap_mt.h
+++ b/servers/physics_2d/physics_2d_server_wrap_mt.h
@@ -223,7 +223,11 @@ public:
 	FUNC2(body_set_applied_torque, RID, real_t);
 	FUNC1RC(real_t, body_get_applied_torque, RID);
 
+	FUNC2(body_add_central_force, RID, const Vector2 &);
 	FUNC3(body_add_force, RID, const Vector2 &, const Vector2 &);
+	FUNC2(body_add_torque, RID, real_t);
+	FUNC2(body_apply_central_impulse, RID, const Vector2 &);
+	FUNC2(body_apply_torque_impulse, RID, real_t);
 	FUNC3(body_apply_impulse, RID, const Vector2 &, const Vector2 &);
 	FUNC2(body_set_axis_velocity, RID, const Vector2 &);