5 files changed, 32 insertions, 3 deletions
diff --git a/servers/physics_2d/body_2d_sw.cpp b/servers/physics_2d/body_2d_sw.cpp
index 97f08a6aa3..12ac0bd1ca 100644
--- a/servers/physics_2d/body_2d_sw.cpp
+++ b/servers/physics_2d/body_2d_sw.cpp
@@ -33,7 +33,7 @@
 
 void Body2DSW::_update_inertia() {
 
-	if (get_space() && !inertia_update_list.in_list())
+	if (!user_inertia && get_space() && !inertia_update_list.in_list())
 		get_space()->body_add_to_inertia_update_list(&inertia_update_list);
 
 }
@@ -48,6 +48,8 @@ void Body2DSW::update_inertias() {
 
 		case Physics2DServer::BODY_MODE_RIGID: {
 
+			if(user_inertia) break;
+
 			//update tensor for allshapes, not the best way but should be somehow OK. (inspired from bullet)
 			float total_area=0;
 
@@ -157,6 +159,15 @@ void Body2DSW::set_param(Physics2DServer::BodyParameter p_param, float p_value)
 			_update_inertia();
 
 		} break;
+		case Physics2DServer::BODY_PARAM_INERTIA: {
+			if(p_value<=0) {
+				user_inertia = false;
+				_update_inertia();
+			} else {
+				user_inertia = true;
+				_inv_inertia = 1.0 / p_value;
+			}
+		} break;
 		case Physics2DServer::BODY_PARAM_GRAVITY_SCALE: {
 			gravity_scale=p_value;
 		} break;
@@ -187,8 +198,7 @@ float Body2DSW::get_param(Physics2DServer::BodyParameter p_param) const {
 			return mass;
 		} break;
 		case Physics2DServer::BODY_PARAM_INERTIA: {
-			if(_inv_inertia == 0) return INFINITY;
-			else return 1.0 / _inv_inertia;
+			return _inv_inertia==0 ? 0 : 1.0 / _inv_inertia;
 		} break;
 		case Physics2DServer::BODY_PARAM_GRAVITY_SCALE: {
 			return gravity_scale;
@@ -669,6 +679,7 @@ Body2DSW::Body2DSW() : CollisionObject2DSW(TYPE_BODY), active_list(this), inerti
 	angular_velocity=0;
 	biased_angular_velocity=0;
 	mass=1;
+	user_inertia=false;
 	_inv_inertia=0;
 	_inv_mass=1;
 	bounce=0;
diff --git a/servers/physics_2d/body_2d_sw.h b/servers/physics_2d/body_2d_sw.h
index 6d9bf3cb03..ed98017629 100644
--- a/servers/physics_2d/body_2d_sw.h
+++ b/servers/physics_2d/body_2d_sw.h
@@ -57,6 +57,7 @@ class Body2DSW : public CollisionObject2DSW {
 
 	real_t _inv_mass;
 	real_t _inv_inertia;
+	bool user_inertia;
 
 	Vector2 gravity;
 	real_t area_linear_damp;
@@ -243,6 +244,11 @@ 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) {
+
+		applied_force += p_force;
+		applied_torque += p_offset.cross(p_force);
+	}
 
 	_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; }
diff --git a/servers/physics_2d/physics_2d_server_sw.cpp b/servers/physics_2d/physics_2d_server_sw.cpp
index c571331498..3796ddd961 100644
--- a/servers/physics_2d/physics_2d_server_sw.cpp
+++ b/servers/physics_2d/physics_2d_server_sw.cpp
@@ -860,6 +860,15 @@ void Physics2DServerSW::body_apply_impulse(RID p_body, const Vector2& p_pos, con
 	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->wakeup();
+};
+
 void Physics2DServerSW::body_set_axis_velocity(RID p_body, const Vector2& p_axis_velocity) {
 
 	Body2DSW *body = body_owner.get(p_body);
diff --git a/servers/physics_2d/physics_2d_server_sw.h b/servers/physics_2d/physics_2d_server_sw.h
index cd4dfc1a8b..6415786803 100644
--- a/servers/physics_2d/physics_2d_server_sw.h
+++ b/servers/physics_2d/physics_2d_server_sw.h
@@ -205,6 +205,8 @@ public:
 	virtual void body_set_applied_torque(RID p_body, float p_torque);
 	virtual float body_get_applied_torque(RID p_body) const;
 
+	virtual void body_add_force(RID p_body, const Vector2& p_offset, const Vector2& p_force);
+
 	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 60f8a4c879..891c45addf 100644
--- a/servers/physics_2d/physics_2d_server_wrap_mt.h
+++ b/servers/physics_2d/physics_2d_server_wrap_mt.h
@@ -205,6 +205,7 @@ public:
 	FUNC2(body_set_applied_torque,RID,float);
 	FUNC1RC(float,body_get_applied_torque,RID);
 
+	FUNC3(body_add_force,RID,const Vector2&,const Vector2&);
 	FUNC3(body_apply_impulse,RID,const Vector2&,const Vector2&);
 	FUNC2(body_set_axis_velocity,RID,const Vector2&);