Mass not set properly for RigidBody3D with custom inertia and center of mass

[Crimzoth]

Godot version

v4.0.3.stable.official [5222a99]

System information

Windows 11 - v4.0.3.stable - Forward+

Issue description

EDIT: Applicable to RigidBody2D in the same way.

If a RigidBody3D uses both custom Center of Mass and custom Inertia and Mass != 1, all forces are scaled by the mass of the body. Using only one custom Center of Mass or custom Inertia results in correct and expected behavior.

For example, if we apply a force = 1N to a body with mass = 2kg it will accelerate with acceleration = 1 m/s^2.
Instead this body should accelerate with acceleration = 0.5 m/s^2.

Cause:
RigidBody3D uses 2 fields to store mass information: mass and _inv_mass. The second is calculated as an inverse of the first in GodotBody3D::update_mass_properties(). The problem arises when RigidBody3D uses both custom Center of Mass and custom Inertia. In such case the body is not added to the list of bodies requiring mass properties update in GodotBody3D::_mass_properties_changed().
This results in mass holding value set by the user and _inv_mass always equal 1, which in turn causes incorrect physics calculations in GodotBody3D::integrate_forces().

I conducted a simple experiment to investigate this behavior.

3 RigidBody3D instances are created:

1. Mass = 1kg, Auto Center of Mass, Auto Inertia
2. Mass = 2kg, Auto Center of Mass, Auto Inertia
3. Mass = 2kg, Custom Center of Mass (0, 0, 0), Custom Inertia (0.2, 0.2, 0.2)

On all 3 bodies acts a force of 1N upward.
All 3 bodies use gravity scale = 0.
Expected behavior would be that body 1 accelerates 2 times faster then 2 and 3. This is not observed! Body 1 does accelerate twice as fast as body 2, but body 3 accelerates equally fast as 1.
Attached images below represent state at the start of the experiment and after few seconds.

This can be confirmed by printing mass and inv_mass of a RigidBody3D during integrate_forces method.

Steps to reproduce

Attached project contains a scene reproducing the experiment described in Issue description.

1. Open attached minimal project
2. Run RigidBodyMassIssue3D_apply_force.tscn
3. Observe how body with custom center of mass and custom inertia accelerates equally fast as body with mass = 1kg.
4. Change mass of body with custom center of mass and custom inertia and rerun the project, observe how the acceleration does not change and is always equal to the first body.

Attached project also contains an alternative scene where the same bodies are subject to gravity.

1. Open attached minimal project
2. Run RigidBodyMassIssue3D_gravity.tscn
3. Under gravity all bodies should fall at the same rate. Observe how the body with custom center of mass and custom inertia accelerates faster then the other
4. Change mass of the body with custom center of mass and custom inertia. Observe how the acceleration is proportional to the mass

Minimal reproduction project

GodotRigidBodyMassIssue.zip