Proper support for custom mass properties in 2D/3D physics bodies

Changes:
-Added support for custom inertia and center of mass in 3D
-Added support for custom center of mass in 2D
-Calculated center of mass from shapes in 2D (same as in 3D)
-Fixed mass properties calculation with disabled shapes in 2D/3D
-Removed first_integration which is not used in 2D and doesn't seem to
make a lot of sense (prevents omit_force_integration to work during the
first frame)
-Support for custom inertia on different axes for RigidBody3D

6 files changed, 72 insertions, 14 deletions

diff --git a/doc/classes/PhysicsDirectBodyState2D.xml b/doc/classes/PhysicsDirectBodyState2D.xml
index 4c6adfca32..8698fbec61 100644
--- a/doc/classes/PhysicsDirectBodyState2D.xml
+++ b/doc/classes/PhysicsDirectBodyState2D.xml
@@ -155,6 +155,9 @@
 		<member name="angular_velocity" type="float" setter="set_angular_velocity" getter="get_angular_velocity">
 			The body's rotational velocity.
 		</member>
+		<member name="center_of_mass" type="Vector2" setter="" getter="get_center_of_mass">
+			The body's center of mass.
+		</member>
 		<member name="inverse_inertia" type="float" setter="" getter="get_inverse_inertia">
 			The inverse of the inertia of the body.
 		</member>
diff --git a/doc/classes/PhysicsDirectBodyState3D.xml b/doc/classes/PhysicsDirectBodyState3D.xml
index 271668e339..4599f86423 100644
--- a/doc/classes/PhysicsDirectBodyState3D.xml
+++ b/doc/classes/PhysicsDirectBodyState3D.xml
@@ -157,6 +157,7 @@
 			The body's rotational velocity.
 		</member>
 		<member name="center_of_mass" type="Vector3" setter="" getter="get_center_of_mass">
+			The body's center of mass.
 		</member>
 		<member name="inverse_inertia" type="Vector3" setter="" getter="get_inverse_inertia">
 			The inverse of the inertia of the body.
diff --git a/doc/classes/PhysicsServer2D.xml b/doc/classes/PhysicsServer2D.xml
index c412c7ec59..b3b7fcd956 100644
--- a/doc/classes/PhysicsServer2D.xml
+++ b/doc/classes/PhysicsServer2D.xml
@@ -373,7 +373,7 @@
 			</description>
 		</method>
 		<method name="body_get_param" qualifiers="const">
-			<return type="float" />
+			<return type="Variant" />
 			<argument index="0" name="body" type="RID" />
 			<argument index="1" name="param" type="int" enum="PhysicsServer2D.BodyParameter" />
 			<description>
@@ -449,6 +449,13 @@
 				Removes a shape from a body. The shape is not deleted, so it can be reused afterwards.
 			</description>
 		</method>
+		<method name="body_reset_mass_properties">
+			<return type="void" />
+			<argument index="0" name="body" type="RID" />
+			<description>
+				Restores the default inertia and center of mass based on shapes to cancel any custom values previously set using [method body_set_param].
+			</description>
+		</method>
 		<method name="body_set_axis_velocity">
 			<return type="void" />
 			<argument index="0" name="body" type="RID" />
@@ -522,7 +529,7 @@
 			<return type="void" />
 			<argument index="0" name="body" type="RID" />
 			<argument index="1" name="param" type="int" enum="PhysicsServer2D.BodyParameter" />
-			<argument index="2" name="value" type="float" />
+			<argument index="2" name="value" type="Variant" />
 			<description>
 				Sets a body parameter. See [enum BodyParameter] for a list of available parameters.
 			</description>
@@ -936,16 +943,19 @@
 		<constant name="BODY_PARAM_INERTIA" value="3" enum="BodyParameter">
 			Constant to set/get a body's inertia.
 		</constant>
-		<constant name="BODY_PARAM_GRAVITY_SCALE" value="4" enum="BodyParameter">
+		<constant name="BODY_PARAM_CENTER_OF_MASS" value="4" enum="BodyParameter">
+			Constant to set/get a body's center of mass.
+		</constant>
+		<constant name="BODY_PARAM_GRAVITY_SCALE" value="5" enum="BodyParameter">
 			Constant to set/get a body's gravity multiplier.
 		</constant>
-		<constant name="BODY_PARAM_LINEAR_DAMP" value="5" enum="BodyParameter">
+		<constant name="BODY_PARAM_LINEAR_DAMP" value="6" enum="BodyParameter">
 			Constant to set/get a body's linear dampening factor.
 		</constant>
-		<constant name="BODY_PARAM_ANGULAR_DAMP" value="6" enum="BodyParameter">
+		<constant name="BODY_PARAM_ANGULAR_DAMP" value="7" enum="BodyParameter">
 			Constant to set/get a body's angular dampening factor.
 		</constant>
-		<constant name="BODY_PARAM_MAX" value="7" enum="BodyParameter">
+		<constant name="BODY_PARAM_MAX" value="8" enum="BodyParameter">
 			Represents the size of the [enum BodyParameter] enum.
 		</constant>
 		<constant name="BODY_STATE_TRANSFORM" value="0" enum="BodyState">
diff --git a/doc/classes/PhysicsServer3D.xml b/doc/classes/PhysicsServer3D.xml
index 46cde02f2c..2fbe84b8b1 100644
--- a/doc/classes/PhysicsServer3D.xml
+++ b/doc/classes/PhysicsServer3D.xml
@@ -347,7 +347,7 @@
 			</description>
 		</method>
 		<method name="body_get_param" qualifiers="const">
-			<return type="float" />
+			<return type="Variant" />
 			<argument index="0" name="body" type="RID" />
 			<argument index="1" name="param" type="int" enum="PhysicsServer3D.BodyParameter" />
 			<description>
@@ -430,6 +430,13 @@
 				Removes a shape from a body. The shape is not deleted, so it can be reused afterwards.
 			</description>
 		</method>
+		<method name="body_reset_mass_properties">
+			<return type="void" />
+			<argument index="0" name="body" type="RID" />
+			<description>
+				Restores the default inertia and center of mass based on shapes to cancel any custom values previously set using [method body_set_param].
+			</description>
+		</method>
 		<method name="body_set_axis_lock">
 			<return type="void" />
 			<argument index="0" name="body" type="RID" />
@@ -511,7 +518,7 @@
 			<return type="void" />
 			<argument index="0" name="body" type="RID" />
 			<argument index="1" name="param" type="int" enum="PhysicsServer3D.BodyParameter" />
-			<argument index="2" name="value" type="float" />
+			<argument index="2" name="value" type="Variant" />
 			<description>
 				Sets a body parameter. A list of available parameters is on the [enum BodyParameter] constants.
 			</description>
@@ -1282,16 +1289,22 @@
 		<constant name="BODY_PARAM_MASS" value="2" enum="BodyParameter">
 			Constant to set/get a body's mass.
 		</constant>
-		<constant name="BODY_PARAM_GRAVITY_SCALE" value="3" enum="BodyParameter">
+		<constant name="BODY_PARAM_INERTIA" value="3" enum="BodyParameter">
+			Constant to set/get a body's inertia.
+		</constant>
+		<constant name="BODY_PARAM_CENTER_OF_MASS" value="4" enum="BodyParameter">
+			Constant to set/get a body's center of mass.
+		</constant>
+		<constant name="BODY_PARAM_GRAVITY_SCALE" value="5" enum="BodyParameter">
 			Constant to set/get a body's gravity multiplier.
 		</constant>
-		<constant name="BODY_PARAM_LINEAR_DAMP" value="4" enum="BodyParameter">
+		<constant name="BODY_PARAM_LINEAR_DAMP" value="6" enum="BodyParameter">
 			Constant to set/get a body's linear dampening factor.
 		</constant>
-		<constant name="BODY_PARAM_ANGULAR_DAMP" value="5" enum="BodyParameter">
+		<constant name="BODY_PARAM_ANGULAR_DAMP" value="7" enum="BodyParameter">
 			Constant to set/get a body's angular dampening factor.
 		</constant>
-		<constant name="BODY_PARAM_MAX" value="6" enum="BodyParameter">
+		<constant name="BODY_PARAM_MAX" value="8" enum="BodyParameter">
 			Represents the size of the [enum BodyParameter] enum.
 		</constant>
 		<constant name="BODY_STATE_TRANSFORM" value="0" enum="BodyState">
diff --git a/doc/classes/RigidBody2D.xml b/doc/classes/RigidBody2D.xml
index 40b5a706c7..0702955495 100644
--- a/doc/classes/RigidBody2D.xml
+++ b/doc/classes/RigidBody2D.xml
@@ -99,6 +99,13 @@
 		<member name="can_sleep" type="bool" setter="set_can_sleep" getter="is_able_to_sleep" default="true">
 			If [code]true[/code], the body can enter sleep mode when there is no movement. See [member sleeping].
 		</member>
+		<member name="center_of_mass" type="Vector2" setter="set_center_of_mass" getter="get_center_of_mass" default="Vector2(0, 0)">
+			The body's custom center of mass, relative to the body's origin position, when [member center_of_mass_mode] is set to [constant CENTER_OF_MASS_MODE_CUSTOM]. This is the balanced point of the body, where applied forces only cause linear acceleration. Applying forces outside of the center of mass causes angular acceleration.
+			When [member center_of_mass_mode] is set to [constant CENTER_OF_MASS_MODE_AUTO] (default value), the center of mass is automatically computed.
+		</member>
+		<member name="center_of_mass_mode" type="int" setter="set_center_of_mass_mode" getter="get_center_of_mass_mode" enum="RigidBody2D.CenterOfMassMode" default="0">
+			Defines the way the body's center of mass is set. See [enum CenterOfMassMode] for possible values.
+		</member>
 		<member name="contact_monitor" type="bool" setter="set_contact_monitor" getter="is_contact_monitor_enabled" default="false">
 			If [code]true[/code], the body will emit signals when it collides with another RigidBody2D. See also [member contacts_reported].
 		</member>
@@ -116,8 +123,9 @@
 		<member name="gravity_scale" type="float" setter="set_gravity_scale" getter="get_gravity_scale" default="1.0">
 			Multiplies the gravity applied to the body. The body's gravity is calculated from the [b]Default Gravity[/b] value in [b]Project &gt; Project Settings &gt; Physics &gt; 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 inertia to return to automatically computing it.
+		<member name="inertia" type="float" setter="set_inertia" getter="get_inertia" default="0.0">
+			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 property allows you to set a custom value.
+			If set to [code]0[/code], inertia is automatically computed (default value).
 		</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].
@@ -202,6 +210,12 @@
 		<constant name="MODE_KINEMATIC" value="3" enum="Mode">
 			Kinematic body mode. The body behaves like a [AnimatableBody2D], and must be moved by code.
 		</constant>
+		<constant name="CENTER_OF_MASS_MODE_AUTO" value="0" enum="CenterOfMassMode">
+			In this mode, the body's center of mass is calculated automatically based on its shapes.
+		</constant>
+		<constant name="CENTER_OF_MASS_MODE_CUSTOM" value="1" enum="CenterOfMassMode">
+			In this mode, the body's center of mass is set through [member center_of_mass]. Defaults to the body's origin position.
+		</constant>
 		<constant name="CCD_MODE_DISABLED" value="0" enum="CCDMode">
 			Continuous collision detection disabled. This is the fastest way to detect body collisions, but can miss small, fast-moving objects.
 		</constant>
diff --git a/doc/classes/RigidBody3D.xml b/doc/classes/RigidBody3D.xml
index 25fb1edde9..1be35b0576 100644
--- a/doc/classes/RigidBody3D.xml
+++ b/doc/classes/RigidBody3D.xml
@@ -102,6 +102,13 @@
 		<member name="can_sleep" type="bool" setter="set_can_sleep" getter="is_able_to_sleep" default="true">
 			If [code]true[/code], the body can enter sleep mode when there is no movement. See [member sleeping].
 		</member>
+		<member name="center_of_mass" type="Vector3" setter="set_center_of_mass" getter="get_center_of_mass" default="Vector3(0, 0, 0)">
+			The body's custom center of mass, relative to the body's origin position, when [member center_of_mass_mode] is set to [constant CENTER_OF_MASS_MODE_CUSTOM]. This is the balanced point of the body, where applied forces only cause linear acceleration. Applying forces outside of the center of mass causes angular acceleration.
+			When [member center_of_mass_mode] is set to [constant CENTER_OF_MASS_MODE_AUTO] (default value), the center of mass is automatically computed.
+		</member>
+		<member name="center_of_mass_mode" type="int" setter="set_center_of_mass_mode" getter="get_center_of_mass_mode" enum="RigidBody3D.CenterOfMassMode" default="0">
+			Defines the way the body's center of mass is set. See [enum CenterOfMassMode] for possible values.
+		</member>
 		<member name="contact_monitor" type="bool" setter="set_contact_monitor" getter="is_contact_monitor_enabled" default="false">
 			If [code]true[/code], the RigidBody3D will emit signals when it collides with another RigidBody3D. See also [member contacts_reported].
 		</member>
@@ -119,6 +126,10 @@
 		<member name="gravity_scale" type="float" setter="set_gravity_scale" getter="get_gravity_scale" default="1.0">
 			This is multiplied by the global 3D gravity setting found in [b]Project &gt; Project Settings &gt; Physics &gt; 3d[/b] to produce RigidBody3D's gravity. For example, a value of 1 will be normal gravity, 2 will apply double gravity, and 0.5 will apply half gravity to this object.
 		</member>
+		<member name="inertia" type="Vector3" setter="set_inertia" getter="get_inertia" default="Vector3(0, 0, 0)">
+			The body's moment of inertia. This is like mass, but for rotation: it determines how much torque it takes to rotate the body on each axis. The moment of inertia is usually computed automatically from the mass and the shapes, but this property allows you to set a custom value.
+			If set to [code]Vector3.ZERO[/code], inertia is automatically computed (default value).
+		</member>
 		<member name="linear_damp" type="float" setter="set_linear_damp" getter="get_linear_damp" default="-1.0">
 			The body's linear damp. Cannot be less than -1.0. If this value is different from -1.0, any linear damp derived from the world or areas will be overridden.
 			See [member ProjectSettings.physics/3d/default_linear_damp] for more details about damping.
@@ -204,5 +215,11 @@
 		<constant name="MODE_KINEMATIC" value="3" enum="Mode">
 			Kinematic body mode. The body behaves like a [AnimatableBody3D], and can only move by user code.
 		</constant>
+		<constant name="CENTER_OF_MASS_MODE_AUTO" value="0" enum="CenterOfMassMode">
+			In this mode, the body's center of mass is calculated automatically based on its shapes.
+		</constant>
+		<constant name="CENTER_OF_MASS_MODE_CUSTOM" value="1" enum="CenterOfMassMode">
+			In this mode, the body's center of mass is set through [member center_of_mass]. Defaults to the body's origin position.
+		</constant>
 	</constants>
 </class>