Merge pull request #31386 from raphael10241024/fix_inertia

Fix custom inertia in physics2d

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].

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);
 }
 

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;
 	}
 

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();

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;