Fix custom inertia in physics2d, closes#30838

5 files changed, 20 insertions, 15 deletions

diff --git a/doc/classes/RigidBody2D.xml b/doc/classes/RigidBody2D.xml
index f280dc81c2..32a1634f77 100644
--- a/doc/classes/RigidBody2D.xml
+++ b/doc/classes/RigidBody2D.xml
@@ -154,7 +154,7 @@
 			Multiplies the gravity applied to the body. The body's gravity is calculated from the [b]Default Gravity[/b] value in [b]Project > Project Settings > Physics > 2d[/b] and/or any additional gravity vector applied by [Area2D]s.
 		</member>
 		<member name="inertia" type="float" setter="set_inertia" getter="get_inertia">
-			The body's moment of inertia. This is like mass, but for rotation: it determines how much torque it takes to rotate the body. The moment of inertia is usually computed automatically from the mass and the shapes, but this function allows you to set a custom value. Set 0 (or negative) inertia to return to automatically computing it.
+			The body's moment of inertia. This is like mass, but for rotation: it determines how much torque it takes to rotate the body. The moment of inertia is usually computed automatically from the mass and the shapes, but this function allows you to set a custom value. Set 0 inertia to return to automatically computing it.
 		</member>
 		<member name="linear_damp" type="float" setter="set_linear_damp" getter="get_linear_damp" default="-1.0">
 			Damps the body's [member linear_velocity]. If [code]-1[/code], the body will use the [b]Default Linear Damp[/b] in [b]Project &gt; Project Settings &gt; Physics &gt; 2d[/b].
diff --git a/scene/2d/physics_body_2d.cpp b/scene/2d/physics_body_2d.cpp
index 49da709a47..0d9df3dae9 100644
--- a/scene/2d/physics_body_2d.cpp
+++ b/scene/2d/physics_body_2d.cpp
@@ -594,7 +594,7 @@ real_t RigidBody2D::get_mass() const {
 
 void RigidBody2D::set_inertia(real_t p_inertia) {
 
-	ERR_FAIL_COND(p_inertia <= 0);
+	ERR_FAIL_COND(p_inertia < 0);
 	Physics2DServer::get_singleton()->body_set_param(get_rid(), Physics2DServer::BODY_PARAM_INERTIA, p_inertia);
 }
 
diff --git a/servers/physics/body_sw.cpp b/servers/physics/body_sw.cpp
index 172a2a3429..a3bb581cb5 100644
--- a/servers/physics/body_sw.cpp
+++ b/servers/physics/body_sw.cpp
@@ -266,6 +266,7 @@ void BodySW::set_mode(PhysicsServer::BodyMode p_mode) {
 
 			_inv_mass = mass > 0 ? (1.0 / mass) : 0;
 			_set_static(false);
+			angular_velocity = Vector3();
 		} break;
 	}
 
diff --git a/servers/physics_2d/body_2d_sw.cpp b/servers/physics_2d/body_2d_sw.cpp
index b9ebd30021..60fb3a3a95 100644
--- a/servers/physics_2d/body_2d_sw.cpp
+++ b/servers/physics_2d/body_2d_sw.cpp
@@ -47,8 +47,10 @@ void Body2DSW::update_inertias() {
 
 		case Physics2DServer::BODY_MODE_RIGID: {
 
-			if (user_inertia) break;
-
+			if (user_inertia) {
+				_inv_inertia = inertia > 0 ? (1.0 / inertia) : 0;
+				break;
+			}
 			//update tensor for allshapes, not the best way but should be somehow OK. (inspired from bullet)
 			real_t total_area = 0;
 
@@ -57,7 +59,7 @@ void Body2DSW::update_inertias() {
 				total_area += get_shape_aabb(i).get_area();
 			}
 
-			real_t _inertia = 0;
+			inertia = 0;
 
 			for (int i = 0; i < get_shape_count(); i++) {
 
@@ -73,15 +75,10 @@ void Body2DSW::update_inertias() {
 
 				Transform2D mtx = get_shape_transform(i);
 				Vector2 scale = mtx.get_scale();
-				_inertia += shape->get_moment_of_inertia(mass, scale) + mass * mtx.get_origin().length_squared();
-				//Rect2 ab = get_shape_aabb(i);
-				//_inertia+=mass*ab.size.dot(ab.size)/12.0f;
+				inertia += shape->get_moment_of_inertia(mass, scale) + mass * mtx.get_origin().length_squared();
 			}
 
-			if (_inertia != 0)
-				_inv_inertia = 1.0 / _inertia;
-			else
-				_inv_inertia = 0.0; //wathever
+			_inv_inertia = inertia > 0 ? (1.0 / inertia) : 0;
 
 			if (mass)
 				_inv_mass = 1.0 / mass;
@@ -160,6 +157,7 @@ void Body2DSW::set_param(Physics2DServer::BodyParameter p_param, real_t p_value)
 				_update_inertia();
 			} else {
 				user_inertia = true;
+				inertia = p_value;
 				_inv_inertia = 1.0 / p_value;
 			}
 		} break;
@@ -194,7 +192,7 @@ real_t Body2DSW::get_param(Physics2DServer::BodyParameter p_param) const {
 			return mass;
 		}
 		case Physics2DServer::BODY_PARAM_INERTIA: {
-			return _inv_inertia == 0 ? 0 : 1.0 / _inv_inertia;
+			return inertia;
 		}
 		case Physics2DServer::BODY_PARAM_GRAVITY_SCALE: {
 			return gravity_scale;
@@ -226,6 +224,7 @@ void Body2DSW::set_mode(Physics2DServer::BodyMode p_mode) {
 
 			_set_inv_transform(get_transform().affine_inverse());
 			_inv_mass = 0;
+			_inv_inertia = 0;
 			_set_static(p_mode == Physics2DServer::BODY_MODE_STATIC);
 			set_active(p_mode == Physics2DServer::BODY_MODE_KINEMATIC && contacts.size());
 			linear_velocity = Vector2();
@@ -237,17 +236,21 @@ void Body2DSW::set_mode(Physics2DServer::BodyMode p_mode) {
 		case Physics2DServer::BODY_MODE_RIGID: {
 
 			_inv_mass = mass > 0 ? (1.0 / mass) : 0;
+			_inv_inertia = inertia > 0 ? (1.0 / inertia) : 0;
 			_set_static(false);
 
 		} break;
 		case Physics2DServer::BODY_MODE_CHARACTER: {
 
 			_inv_mass = mass > 0 ? (1.0 / mass) : 0;
+			_inv_inertia = 0;
 			_set_static(false);
+			angular_velocity = 0;
 		} break;
 	}
-
-	_update_inertia();
+	if (p_mode == Physics2DServer::BODY_MODE_RIGID && _inv_inertia == 0) {
+		_update_inertia();
+	}
 	/*
 	if (get_space())
 		_update_queries();
diff --git a/servers/physics_2d/body_2d_sw.h b/servers/physics_2d/body_2d_sw.h
index 8c64dc9230..5df184c894 100644
--- a/servers/physics_2d/body_2d_sw.h
+++ b/servers/physics_2d/body_2d_sw.h
@@ -52,6 +52,7 @@ class Body2DSW : public CollisionObject2DSW {
 	real_t gravity_scale;
 
 	real_t mass;
+	real_t inertia;
 	real_t bounce;
 	real_t friction;