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

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>

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.

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">

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">

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>

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>

scene/2d/physics_body_2d.cpp

@@ -560,11 +560,53 @@ real_t RigidBody2D::get_mass() const {
 
 void RigidBody2D::set_inertia(real_t p_inertia) {
 	ERR_FAIL_COND(p_inertia < 0);
-	PhysicsServer2D::get_singleton()->body_set_param(get_rid(), PhysicsServer2D::BODY_PARAM_INERTIA, p_inertia);
+	inertia = p_inertia;
+	PhysicsServer2D::get_singleton()->body_set_param(get_rid(), PhysicsServer2D::BODY_PARAM_INERTIA, inertia);
 }
 
 real_t RigidBody2D::get_inertia() const {
-	return PhysicsServer2D::get_singleton()->body_get_param(get_rid(), PhysicsServer2D::BODY_PARAM_INERTIA);
+	return inertia;
+}
+
+void RigidBody2D::set_center_of_mass_mode(CenterOfMassMode p_mode) {
+	if (center_of_mass_mode == p_mode) {
+		return;
+	}
+
+	center_of_mass_mode = p_mode;
+
+	switch (center_of_mass_mode) {
+		case CENTER_OF_MASS_MODE_AUTO: {
+			center_of_mass = Vector2();
+			PhysicsServer2D::get_singleton()->body_reset_mass_properties(get_rid());
+			if (inertia != 0.0) {
+				PhysicsServer2D::get_singleton()->body_set_param(get_rid(), PhysicsServer2D::BODY_PARAM_INERTIA, inertia);
+			}
+		} break;
+
+		case CENTER_OF_MASS_MODE_CUSTOM: {
+			PhysicsServer2D::get_singleton()->body_set_param(get_rid(), PhysicsServer2D::BODY_PARAM_CENTER_OF_MASS, center_of_mass);
+		} break;
+	}
+}
+
+RigidBody2D::CenterOfMassMode RigidBody2D::get_center_of_mass_mode() const {
+	return center_of_mass_mode;
+}
+
+void RigidBody2D::set_center_of_mass(const Vector2 &p_center_of_mass) {
+	if (center_of_mass == p_center_of_mass) {
+		return;
+	}
+
+	ERR_FAIL_COND(center_of_mass_mode != CENTER_OF_MASS_MODE_CUSTOM);
+	center_of_mass = p_center_of_mass;
+
+	PhysicsServer2D::get_singleton()->body_set_param(get_rid(), PhysicsServer2D::BODY_PARAM_CENTER_OF_MASS, center_of_mass);
+}
+
+const Vector2 &RigidBody2D::get_center_of_mass() const {
+	return center_of_mass;
 }
 
 void RigidBody2D::set_physics_material_override(const Ref<PhysicsMaterial> &p_physics_material_override) {
@@ -818,6 +860,12 @@ void RigidBody2D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("get_inertia"), &RigidBody2D::get_inertia);
 	ClassDB::bind_method(D_METHOD("set_inertia", "inertia"), &RigidBody2D::set_inertia);
 
+	ClassDB::bind_method(D_METHOD("set_center_of_mass_mode", "mode"), &RigidBody2D::set_center_of_mass_mode);
+	ClassDB::bind_method(D_METHOD("get_center_of_mass_mode"), &RigidBody2D::get_center_of_mass_mode);
+
+	ClassDB::bind_method(D_METHOD("set_center_of_mass", "center_of_mass"), &RigidBody2D::set_center_of_mass);
+	ClassDB::bind_method(D_METHOD("get_center_of_mass"), &RigidBody2D::get_center_of_mass);
+
 	ClassDB::bind_method(D_METHOD("set_physics_material_override", "physics_material_override"), &RigidBody2D::set_physics_material_override);
 	ClassDB::bind_method(D_METHOD("get_physics_material_override"), &RigidBody2D::get_physics_material_override);
 
@@ -874,8 +922,11 @@ void RigidBody2D::_bind_methods() {
 	GDVIRTUAL_BIND(_integrate_forces, "state");
 
 	ADD_PROPERTY(PropertyInfo(Variant::INT, "mode", PROPERTY_HINT_ENUM, "Dynamic,Static,DynamicLocked,Kinematic"), "set_mode", "get_mode");
-	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "mass", PROPERTY_HINT_RANGE, "0.01,65535,0.01,exp"), "set_mass", "get_mass");
-	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "inertia", PROPERTY_HINT_RANGE, "0.01,65535,0.01,exp", PROPERTY_USAGE_NONE), "set_inertia", "get_inertia");
+	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "mass", PROPERTY_HINT_RANGE, "0.01,1000,0.01,or_greater,exp"), "set_mass", "get_mass");
+	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "inertia", PROPERTY_HINT_RANGE, "0,1000,0.01,or_greater,exp"), "set_inertia", "get_inertia");
+	ADD_PROPERTY(PropertyInfo(Variant::INT, "center_of_mass_mode", PROPERTY_HINT_ENUM, "Auto,Custom", PROPERTY_USAGE_DEFAULT | PROPERTY_USAGE_UPDATE_ALL_IF_MODIFIED), "set_center_of_mass_mode", "get_center_of_mass_mode");
+	ADD_PROPERTY(PropertyInfo(Variant::VECTOR2, "center_of_mass", PROPERTY_HINT_RANGE, "-10,10,0.01,or_lesser,or_greater"), "set_center_of_mass", "get_center_of_mass");
+	ADD_LINKED_PROPERTY("center_of_mass_mode", "center_of_mass");
 	ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "physics_material_override", PROPERTY_HINT_RESOURCE_TYPE, "PhysicsMaterial"), "set_physics_material_override", "get_physics_material_override");
 	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "gravity_scale", PROPERTY_HINT_RANGE, "-128,128,0.01"), "set_gravity_scale", "get_gravity_scale");
 	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "custom_integrator"), "set_use_custom_integrator", "is_using_custom_integrator");
@@ -905,11 +956,22 @@ void RigidBody2D::_bind_methods() {
 	BIND_ENUM_CONSTANT(MODE_DYNAMIC_LOCKED);
 	BIND_ENUM_CONSTANT(MODE_KINEMATIC);
 
+	BIND_ENUM_CONSTANT(CENTER_OF_MASS_MODE_AUTO);
+	BIND_ENUM_CONSTANT(CENTER_OF_MASS_MODE_CUSTOM);
+
 	BIND_ENUM_CONSTANT(CCD_MODE_DISABLED);
 	BIND_ENUM_CONSTANT(CCD_MODE_CAST_RAY);
 	BIND_ENUM_CONSTANT(CCD_MODE_CAST_SHAPE);
 }
 
+void RigidBody2D::_validate_property(PropertyInfo &property) const {
+	if (center_of_mass_mode != CENTER_OF_MASS_MODE_CUSTOM) {
+		if (property.name == "center_of_mass") {
+			property.usage = PROPERTY_USAGE_NOEDITOR | PROPERTY_USAGE_INTERNAL;
+		}
+	}
+}
+
 RigidBody2D::RigidBody2D() :
 		PhysicsBody2D(PhysicsServer2D::BODY_MODE_DYNAMIC) {
 	PhysicsServer2D::get_singleton()->body_set_state_sync_callback(get_rid(), this, _body_state_changed_callback);

scene/2d/physics_body_2d.h

@@ -124,6 +124,11 @@ public:
 		MODE_KINEMATIC,
 	};
 
+	enum CenterOfMassMode {
+		CENTER_OF_MASS_MODE_AUTO,
+		CENTER_OF_MASS_MODE_CUSTOM,
+	};
+
 	enum CCDMode {
 		CCD_MODE_DISABLED,
 		CCD_MODE_CAST_RAY,
@@ -135,6 +140,10 @@ private:
 	Mode mode = MODE_DYNAMIC;
 
 	real_t mass = 1.0;
+	real_t inertia = 0.0;
+	CenterOfMassMode center_of_mass_mode = CENTER_OF_MASS_MODE_AUTO;
+	Vector2 center_of_mass;
+
 	Ref<PhysicsMaterial> physics_material_override;
 	real_t gravity_scale = 1.0;
 	real_t linear_damp = -1.0;
@@ -198,6 +207,8 @@ protected:
 	void _notification(int p_what);
 	static void _bind_methods();
 
+	virtual void _validate_property(PropertyInfo &property) const override;
+
 	GDVIRTUAL1(_integrate_forces, PhysicsDirectBodyState2D *)
 
 public:
@@ -210,6 +221,12 @@ public:
 	void set_inertia(real_t p_inertia);
 	real_t get_inertia() const;
 
+	void set_center_of_mass_mode(CenterOfMassMode p_mode);
+	CenterOfMassMode get_center_of_mass_mode() const;
+
+	void set_center_of_mass(const Vector2 &p_center_of_mass);
+	const Vector2 &get_center_of_mass() const;
+
 	void set_physics_material_override(const Ref<PhysicsMaterial> &p_physics_material_override);
 	Ref<PhysicsMaterial> get_physics_material_override() const;
 
@@ -274,6 +291,7 @@ private:
 };
 
 VARIANT_ENUM_CAST(RigidBody2D::Mode);
+VARIANT_ENUM_CAST(RigidBody2D::CenterOfMassMode);
 VARIANT_ENUM_CAST(RigidBody2D::CCDMode);
 
 class CharacterBody2D : public PhysicsBody2D {

scene/3d/physics_body_3d.cpp

@@ -631,6 +631,60 @@ real_t RigidBody3D::get_mass() const {
 	return mass;
 }
 
+void RigidBody3D::set_inertia(const Vector3 &p_inertia) {
+	ERR_FAIL_COND(p_inertia.x < 0);
+	ERR_FAIL_COND(p_inertia.y < 0);
+	ERR_FAIL_COND(p_inertia.z < 0);
+
+	inertia = p_inertia;
+	PhysicsServer3D::get_singleton()->body_set_param(get_rid(), PhysicsServer3D::BODY_PARAM_INERTIA, inertia);
+}
+
+const Vector3 &RigidBody3D::get_inertia() const {
+	return inertia;
+}
+
+void RigidBody3D::set_center_of_mass_mode(CenterOfMassMode p_mode) {
+	if (center_of_mass_mode == p_mode) {
+		return;
+	}
+
+	center_of_mass_mode = p_mode;
+
+	switch (center_of_mass_mode) {
+		case CENTER_OF_MASS_MODE_AUTO: {
+			center_of_mass = Vector3();
+			PhysicsServer3D::get_singleton()->body_reset_mass_properties(get_rid());
+			if (inertia != Vector3()) {
+				PhysicsServer3D::get_singleton()->body_set_param(get_rid(), PhysicsServer3D::BODY_PARAM_INERTIA, inertia);
+			}
+		} break;
+
+		case CENTER_OF_MASS_MODE_CUSTOM: {
+			PhysicsServer3D::get_singleton()->body_set_param(get_rid(), PhysicsServer3D::BODY_PARAM_CENTER_OF_MASS, center_of_mass);
+		} break;
+	}
+}
+
+RigidBody3D::CenterOfMassMode RigidBody3D::get_center_of_mass_mode() const {
+	return center_of_mass_mode;
+}
+
+void RigidBody3D::set_center_of_mass(const Vector3 &p_center_of_mass) {
+	if (center_of_mass == p_center_of_mass) {
+		return;
+	}
+
+	ERR_FAIL_COND(center_of_mass_mode != CENTER_OF_MASS_MODE_CUSTOM);
+	center_of_mass = p_center_of_mass;
+
+	PhysicsServer3D::get_singleton()->body_set_param(get_rid(), PhysicsServer3D::BODY_PARAM_CENTER_OF_MASS, center_of_mass);
+}
+
+const Vector3 &RigidBody3D::get_center_of_mass() const {
+	return center_of_mass;
+}
+
 void RigidBody3D::set_physics_material_override(const Ref<PhysicsMaterial> &p_physics_material_override) {
 	if (physics_material_override.is_valid()) {
 		if (physics_material_override->is_connected(CoreStringNames::get_singleton()->changed, callable_mp(this, &RigidBody3D::_reload_physics_characteristics))) {
@@ -851,6 +905,15 @@ void RigidBody3D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("set_mass", "mass"), &RigidBody3D::set_mass);
 	ClassDB::bind_method(D_METHOD("get_mass"), &RigidBody3D::get_mass);
 
+	ClassDB::bind_method(D_METHOD("set_inertia", "inertia"), &RigidBody3D::set_inertia);
+	ClassDB::bind_method(D_METHOD("get_inertia"), &RigidBody3D::get_inertia);
+
+	ClassDB::bind_method(D_METHOD("set_center_of_mass_mode", "mode"), &RigidBody3D::set_center_of_mass_mode);
+	ClassDB::bind_method(D_METHOD("get_center_of_mass_mode"), &RigidBody3D::get_center_of_mass_mode);
+
+	ClassDB::bind_method(D_METHOD("set_center_of_mass", "center_of_mass"), &RigidBody3D::set_center_of_mass);
+	ClassDB::bind_method(D_METHOD("get_center_of_mass"), &RigidBody3D::get_center_of_mass);
+
 	ClassDB::bind_method(D_METHOD("set_physics_material_override", "physics_material_override"), &RigidBody3D::set_physics_material_override);
 	ClassDB::bind_method(D_METHOD("get_physics_material_override"), &RigidBody3D::get_physics_material_override);
 
@@ -904,7 +967,11 @@ void RigidBody3D::_bind_methods() {
 	GDVIRTUAL_BIND(_integrate_forces, "state");
 
 	ADD_PROPERTY(PropertyInfo(Variant::INT, "mode", PROPERTY_HINT_ENUM, "Dynamic,Static,DynamicLocked,Kinematic"), "set_mode", "get_mode");
-	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "mass", PROPERTY_HINT_RANGE, "0.01,65535,0.01,exp"), "set_mass", "get_mass");
+	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "mass", PROPERTY_HINT_RANGE, "0.01,1000,0.01,or_greater,exp"), "set_mass", "get_mass");
+	ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "inertia", PROPERTY_HINT_RANGE, "0,1000,0.01,or_greater,exp"), "set_inertia", "get_inertia");
+	ADD_PROPERTY(PropertyInfo(Variant::INT, "center_of_mass_mode", PROPERTY_HINT_ENUM, "Auto,Custom", PROPERTY_USAGE_DEFAULT | PROPERTY_USAGE_UPDATE_ALL_IF_MODIFIED), "set_center_of_mass_mode", "get_center_of_mass_mode");
+	ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "center_of_mass", PROPERTY_HINT_RANGE, "-10,10,0.01,or_lesser,or_greater"), "set_center_of_mass", "get_center_of_mass");
+	ADD_LINKED_PROPERTY("center_of_mass_mode", "center_of_mass");
 	ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "physics_material_override", PROPERTY_HINT_RESOURCE_TYPE, "PhysicsMaterial"), "set_physics_material_override", "get_physics_material_override");
 	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "gravity_scale", PROPERTY_HINT_RANGE, "-128,128,0.01"), "set_gravity_scale", "get_gravity_scale");
 	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "custom_integrator"), "set_use_custom_integrator", "is_using_custom_integrator");
@@ -930,6 +997,17 @@ void RigidBody3D::_bind_methods() {
 	BIND_ENUM_CONSTANT(MODE_STATIC);
 	BIND_ENUM_CONSTANT(MODE_DYNAMIC_LOCKED);
 	BIND_ENUM_CONSTANT(MODE_KINEMATIC);
+
+	BIND_ENUM_CONSTANT(CENTER_OF_MASS_MODE_AUTO);
+	BIND_ENUM_CONSTANT(CENTER_OF_MASS_MODE_CUSTOM);
+}
+
+void RigidBody3D::_validate_property(PropertyInfo &property) const {
+	if (center_of_mass_mode != CENTER_OF_MASS_MODE_CUSTOM) {
+		if (property.name == "center_of_mass") {
+			property.usage = PROPERTY_USAGE_NOEDITOR | PROPERTY_USAGE_INTERNAL;
+		}
+	}
 }
 
 RigidBody3D::RigidBody3D() :
@@ -2207,7 +2285,7 @@ void PhysicalBone3D::_bind_methods() {
 
 	ADD_PROPERTY(PropertyInfo(Variant::TRANSFORM3D, "body_offset"), "set_body_offset", "get_body_offset");
 
-	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "mass", PROPERTY_HINT_RANGE, "0.01,65535,0.01,exp"), "set_mass", "get_mass");
+	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "mass", PROPERTY_HINT_RANGE, "0.01,1000,0.01,or_greater,exp"), "set_mass", "get_mass");
 	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "friction", PROPERTY_HINT_RANGE, "0,1,0.01"), "set_friction", "get_friction");
 	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "bounce", PROPERTY_HINT_RANGE, "0,1,0.01"), "set_bounce", "get_bounce");
 	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "gravity_scale", PROPERTY_HINT_RANGE, "-10,10,0.01"), "set_gravity_scale", "get_gravity_scale");

scene/3d/physics_body_3d.h

@@ -140,6 +140,11 @@ public:
 		MODE_KINEMATIC,
 	};
 
+	enum CenterOfMassMode {
+		CENTER_OF_MASS_MODE_AUTO,
+		CENTER_OF_MASS_MODE_CUSTOM,
+	};
+
 	GDVIRTUAL1(_integrate_forces, PhysicsDirectBodyState3D *)
 
 protected:
@@ -147,6 +152,10 @@ protected:
 	Mode mode = MODE_DYNAMIC;
 
 	real_t mass = 1.0;
+	Vector3 inertia;
+	CenterOfMassMode center_of_mass_mode = CENTER_OF_MASS_MODE_AUTO;
+	Vector3 center_of_mass;
+
 	Ref<PhysicsMaterial> physics_material_override;
 
 	Vector3 linear_velocity;
@@ -210,6 +219,8 @@ protected:
 	void _notification(int p_what);
 	static void _bind_methods();
 
+	virtual void _validate_property(PropertyInfo &property) const override;
+
 public:
 	void set_mode(Mode p_mode);
 	Mode get_mode() const;
@@ -219,6 +230,15 @@ public:
 
 	virtual real_t get_inverse_mass() const override { return 1.0 / mass; }
 
+	void set_inertia(const Vector3 &p_inertia);
+	const Vector3 &get_inertia() const;
+
+	void set_center_of_mass_mode(CenterOfMassMode p_mode);
+	CenterOfMassMode get_center_of_mass_mode() const;
+
+	void set_center_of_mass(const Vector3 &p_center_of_mass);
+	const Vector3 &get_center_of_mass() const;
+
 	void set_physics_material_override(const Ref<PhysicsMaterial> &p_physics_material_override);
 	Ref<PhysicsMaterial> get_physics_material_override() const;
 
@@ -279,6 +299,7 @@ private:
 };
 
 VARIANT_ENUM_CAST(RigidBody3D::Mode);
+VARIANT_ENUM_CAST(RigidBody3D::CenterOfMassMode);
 
 class KinematicCollision3D;
 

servers/physics_2d/body_2d_sw.cpp

@@ -34,47 +34,72 @@
 #include "body_direct_state_2d_sw.h"
 #include "space_2d_sw.h"
 
-void Body2DSW::_update_inertia() {
-	if (!user_inertia && get_space() && !inertia_update_list.in_list()) {
-		get_space()->body_add_to_inertia_update_list(&inertia_update_list);
+void Body2DSW::_mass_properties_changed() {
+	if (get_space() && !mass_properties_update_list.in_list() && (calculate_inertia || calculate_center_of_mass)) {
+		get_space()->body_add_to_mass_properties_update_list(&mass_properties_update_list);
 	}
 }
 
-void Body2DSW::update_inertias() {
+void Body2DSW::update_mass_properties() {
 	//update shapes and motions
 
 	switch (mode) {
 		case PhysicsServer2D::BODY_MODE_DYNAMIC: {
-			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;
-
 			for (int i = 0; i < get_shape_count(); i++) {
+				if (is_shape_disabled(i)) {
+					continue;
+				}
 				total_area += get_shape_aabb(i).get_area();
 			}
 
-			inertia = 0;
+			if (calculate_center_of_mass) {
+				// We have to recompute the center of mass.
+				center_of_mass = Vector2();
 
-			for (int i = 0; i < get_shape_count(); i++) {
-				if (is_shape_disabled(i)) {
-					continue;
+				if (total_area != 0.0) {
+					for (int i = 0; i < get_shape_count(); i++) {
+						if (is_shape_disabled(i)) {
+							continue;
+						}
+
+						real_t area = get_shape_aabb(i).get_area();
+
+						real_t mass = area * this->mass / total_area;
+
+						// NOTE: we assume that the shape origin is also its center of mass.
+						center_of_mass += mass * get_shape_transform(i).get_origin();
+					}
+
+					center_of_mass /= mass;
 				}
+			}
+
+			if (calculate_inertia) {
+				inertia = 0;
 
-				const Shape2DSW *shape = get_shape(i);
+				for (int i = 0; i < get_shape_count(); i++) {
+					if (is_shape_disabled(i)) {
+						continue;
+					}
 
-				real_t area = get_shape_aabb(i).get_area();
+					const Shape2DSW *shape = get_shape(i);
 
-				real_t mass = area * this->mass / total_area;
+					real_t area = get_shape_aabb(i).get_area();
+					if (area == 0.0) {
+						continue;
+					}
 
-				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();
+					real_t mass = area * this->mass / total_area;
+
+					Transform2D mtx = get_shape_transform(i);
+					Vector2 scale = mtx.get_scale();
+					Vector2 shape_origin = mtx.get_origin() - center_of_mass;
+					inertia += shape->get_moment_of_inertia(mass, scale) + mass * shape_origin.length_squared();
+				}
 			}
 
-			_inv_inertia = inertia > 0 ? (1.0 / inertia) : 0;
+			_inv_inertia = inertia > 0.0 ? (1.0 / inertia) : 0.0;
 
 			if (mass) {
 				_inv_mass = 1.0 / mass;
@@ -94,9 +119,12 @@ void Body2DSW::update_inertias() {
 
 		} break;
 	}
-	//_update_inertia_tensor();
+}
 
-	//_update_shapes();
+void Body2DSW::reset_mass_properties() {
+	calculate_inertia = true;
+	calculate_center_of_mass = true;
+	_mass_properties_changed();
 }
 
 void Body2DSW::set_active(bool p_active) {
@@ -118,7 +146,7 @@ void Body2DSW::set_active(bool p_active) {
 	}
 }
 
-void Body2DSW::set_param(PhysicsServer2D::BodyParameter p_param, real_t p_value) {
+void Body2DSW::set_param(PhysicsServer2D::BodyParameter p_param, const Variant &p_value) {
 	switch (p_param) {
 		case PhysicsServer2D::BODY_PARAM_BOUNCE: {
 			bounce = p_value;
@@ -127,21 +155,32 @@ void Body2DSW::set_param(PhysicsServer2D::BodyParameter p_param, real_t p_value)
 			friction = p_value;
 		} break;
 		case PhysicsServer2D::BODY_PARAM_MASS: {
-			ERR_FAIL_COND(p_value <= 0);
-			mass = p_value;
-			_update_inertia();
-
+			real_t mass_value = p_value;
+			ERR_FAIL_COND(mass_value <= 0);
+			mass = mass_value;
+			if (mode >= PhysicsServer2D::BODY_MODE_DYNAMIC) {
+				_mass_properties_changed();
+			}
 		} break;
 		case PhysicsServer2D::BODY_PARAM_INERTIA: {
-			if (p_value <= 0) {
-				user_inertia = false;
-				_update_inertia();
+			real_t inertia_value = p_value;
+			if (inertia_value <= 0.0) {
+				calculate_inertia = true;
+				if (mode == PhysicsServer2D::BODY_MODE_DYNAMIC) {
+					_mass_properties_changed();
+				}
 			} else {
-				user_inertia = true;
-				inertia = p_value;
-				_inv_inertia = 1.0 / p_value;
+				calculate_inertia = false;
+				inertia = inertia_value;
+				if (mode == PhysicsServer2D::BODY_MODE_DYNAMIC) {
+					_inv_inertia = 1.0 / inertia;
+				}
 			}
 		} break;
+		case PhysicsServer2D::BODY_PARAM_CENTER_OF_MASS: {
+			calculate_center_of_mass = false;
+			center_of_mass = p_value;
+		} break;
 		case PhysicsServer2D::BODY_PARAM_GRAVITY_SCALE: {
 			gravity_scale = p_value;
 		} break;
@@ -156,7 +195,7 @@ void Body2DSW::set_param(PhysicsServer2D::BodyParameter p_param, real_t p_value)
 	}
 }
 
-real_t Body2DSW::get_param(PhysicsServer2D::BodyParameter p_param) const {
+Variant Body2DSW::get_param(PhysicsServer2D::BodyParameter p_param) const {
 	switch (p_param) {
 		case PhysicsServer2D::BODY_PARAM_BOUNCE: {
 			return bounce;
@@ -170,6 +209,9 @@ real_t Body2DSW::get_param(PhysicsServer2D::BodyParameter p_param) const {
 		case PhysicsServer2D::BODY_PARAM_INERTIA: {
 			return inertia;
 		}
+		case PhysicsServer2D::BODY_PARAM_CENTER_OF_MASS: {
+			return center_of_mass;
+		}
 		case PhysicsServer2D::BODY_PARAM_GRAVITY_SCALE: {
 			return gravity_scale;
 		}
@@ -207,7 +249,10 @@ void Body2DSW::set_mode(PhysicsServer2D::BodyMode p_mode) {
 		} break;
 		case PhysicsServer2D::BODY_MODE_DYNAMIC: {
 			_inv_mass = mass > 0 ? (1.0 / mass) : 0;
-			_inv_inertia = inertia > 0 ? (1.0 / inertia) : 0;
+			if (!calculate_inertia) {
+				_inv_inertia = 1.0 / inertia;
+			}
+			_mass_properties_changed();
 			_set_static(false);
 			set_active(true);
 
@@ -215,18 +260,11 @@ void Body2DSW::set_mode(PhysicsServer2D::BodyMode p_mode) {
 		case PhysicsServer2D::BODY_MODE_DYNAMIC_LOCKED: {
 			_inv_mass = mass > 0 ? (1.0 / mass) : 0;
 			_inv_inertia = 0;
+			angular_velocity = 0;
 			_set_static(false);
 			set_active(true);
-			angular_velocity = 0;
-		} break;
-	}
-	if (p_mode == PhysicsServer2D::BODY_MODE_DYNAMIC && _inv_inertia == 0) {
-		_update_inertia();
+		}
 	}
-	/*
-	if (get_space())
-		_update_queries();
-	*/
 }
 
 PhysicsServer2D::BodyMode Body2DSW::get_mode() const {
@@ -234,7 +272,7 @@ PhysicsServer2D::BodyMode Body2DSW::get_mode() const {
 }
 
 void Body2DSW::_shapes_changed() {
-	_update_inertia();
+	_mass_properties_changed();
 	wakeup_neighbours();
 }
 
@@ -298,7 +336,7 @@ void Body2DSW::set_state(PhysicsServer2D::BodyState p_state, const Variant &p_va
 		} break;
 		case PhysicsServer2D::BODY_STATE_CAN_SLEEP: {
 			can_sleep = p_variant;
-			if (mode == PhysicsServer2D::BODY_MODE_DYNAMIC && !active && !can_sleep) {
+			if (mode >= PhysicsServer2D::BODY_MODE_DYNAMIC && !active && !can_sleep) {
 				set_active(true);
 			}
 
@@ -332,8 +370,8 @@ void Body2DSW::set_space(Space2DSW *p_space) {
 	if (get_space()) {
 		wakeup_neighbours();
 
-		if (inertia_update_list.in_list()) {
-			get_space()->body_remove_from_inertia_update_list(&inertia_update_list);
+		if (mass_properties_update_list.in_list()) {
+			get_space()->body_remove_from_mass_properties_update_list(&mass_properties_update_list);
 		}
 		if (active_list.in_list()) {
 			get_space()->body_remove_from_active_list(&active_list);
@@ -346,13 +384,11 @@ void Body2DSW::set_space(Space2DSW *p_space) {
 	_set_space(p_space);
 
 	if (get_space()) {
-		_update_inertia();
+		_mass_properties_changed();
 		if (active) {
 			get_space()->body_add_to_active_list(&active_list);
 		}
 	}
-
-	first_integration = false;
 }
 
 void Body2DSW::_compute_area_gravity_and_damping(const Area2DSW *p_area) {
@@ -446,7 +482,7 @@ void Body2DSW::integrate_forces(real_t p_step) {
 		*/
 
 	} else {
-		if (!omit_force_integration && !first_integration) {
+		if (!omit_force_integration) {
 			//overridden by direct state query
 
 			Vector2 force = gravity * mass;
@@ -480,7 +516,6 @@ void Body2DSW::integrate_forces(real_t p_step) {
 
 	//motion=linear_velocity*p_step;
 
-	first_integration = false;
 	biased_angular_velocity = 0;
 	biased_linear_velocity = Vector2();
 
@@ -517,14 +552,22 @@ void Body2DSW::integrate_velocities(real_t p_step) {
 	real_t angle = get_transform().get_rotation() + total_angular_velocity * p_step;
 	Vector2 pos = get_transform().get_origin() + total_linear_velocity * p_step;
 
+	real_t center_of_mass_distance = center_of_mass.length();
+	if (center_of_mass_distance > CMP_EPSILON) {
+		// Calculate displacement due to center of mass offset.
+		real_t prev_angle = get_transform().get_rotation();
+		real_t angle_base = Math::atan2(center_of_mass.y, center_of_mass.x);
+		Vector2 point1(Math::cos(angle_base + prev_angle), Math::sin(angle_base + prev_angle));
+		Vector2 point2(Math::cos(angle_base + angle), Math::sin(angle_base + angle));
+		pos += center_of_mass_distance * (point1 - point2);
+	}
+
 	_set_transform(Transform2D(angle, pos), continuous_cd_mode == PhysicsServer2D::CCD_MODE_DISABLED);
 	_set_inv_transform(get_transform().inverse());
 
 	if (continuous_cd_mode != PhysicsServer2D::CCD_MODE_DISABLED) {
 		new_transform = get_transform();
 	}
-
-	//_update_inertia_tensor();
 }
 
 void Body2DSW::wakeup_neighbours() {
@@ -538,7 +581,7 @@ void Body2DSW::wakeup_neighbours() {
 				continue;
 			}
 			Body2DSW *b = n[i];
-			if (b->mode != PhysicsServer2D::BODY_MODE_DYNAMIC) {
+			if (b->mode < PhysicsServer2D::BODY_MODE_DYNAMIC) {
 				continue;
 			}
 
@@ -619,33 +662,9 @@ PhysicsDirectBodyState2DSW *Body2DSW::get_direct_state() {
 Body2DSW::Body2DSW() :
 		CollisionObject2DSW(TYPE_BODY),
 		active_list(this),
-		inertia_update_list(this),
+		mass_properties_update_list(this),
 		direct_state_query_list(this) {
-	mode = PhysicsServer2D::BODY_MODE_DYNAMIC;
-	active = true;
-	mass = 1;
-	inertia = 0;
-	user_inertia = false;
-	_inv_inertia = 0;
-	_inv_mass = 1;
-	bounce = 0;
-	friction = 1;
-	omit_force_integration = false;
-	applied_torque = 0;
-	island_step = 0;
 	_set_static(false);
-	first_time_kinematic = false;
-	linear_damp = -1;
-	angular_damp = -1;
-	area_angular_damp = 0;
-	area_linear_damp = 0;
-	contact_count = 0;
-	gravity_scale = 1.0;
-	first_integration = false;
-
-	still_time = 0;
-	continuous_cd_mode = PhysicsServer2D::CCD_MODE_DISABLED;
-	can_sleep = true;
 }
 
 Body2DSW::~Body2DSW() {

servers/physics_2d/body_2d_sw.h

@@ -41,7 +41,7 @@ class Constraint2DSW;
 class PhysicsDirectBodyState2DSW;
 
 class Body2DSW : public CollisionObject2DSW {
-	PhysicsServer2D::BodyMode mode;
+	PhysicsServer2D::BodyMode mode = PhysicsServer2D::BODY_MODE_DYNAMIC;
 
 	Vector2 biased_linear_velocity;
 	real_t biased_angular_velocity = 0.0;
@@ -52,40 +52,44 @@ class Body2DSW : public CollisionObject2DSW {
 	Vector2 constant_linear_velocity;
 	real_t constant_angular_velocity = 0.0;
 
-	real_t linear_damp;
-	real_t angular_damp;
-	real_t gravity_scale;
+	real_t linear_damp = -1.0;
+	real_t angular_damp = -1.0;
+	real_t gravity_scale = 1.0;
 
-	real_t mass;
-	real_t inertia;
-	real_t bounce;
-	real_t friction;
+	real_t bounce = 0.0;
+	real_t friction = 1.0;
 
-	real_t _inv_mass;
-	real_t _inv_inertia;
-	bool user_inertia;
+	real_t mass = 1.0;
+	real_t _inv_mass = 1.0;
+
+	real_t inertia = 0.0;
+	real_t _inv_inertia = 0.0;
+
+	Vector2 center_of_mass;
+
+	bool calculate_inertia = true;
+	bool calculate_center_of_mass = true;
 
 	Vector2 gravity;
-	real_t area_linear_damp;
-	real_t area_angular_damp;
+	real_t area_linear_damp = 0.0;
+	real_t area_angular_damp = 0.0;
 
-	real_t still_time;
+	real_t still_time = 0.0;
 
 	Vector2 applied_force;
-	real_t applied_torque;
+	real_t applied_torque = 0.0;
 
 	SelfList<Body2DSW> active_list;
-	SelfList<Body2DSW> inertia_update_list;
+	SelfList<Body2DSW> mass_properties_update_list;
 	SelfList<Body2DSW> direct_state_query_list;
 
 	VSet<RID> exceptions;
-	PhysicsServer2D::CCDMode continuous_cd_mode;
-	bool omit_force_integration;
-	bool active;
-	bool can_sleep;
-	bool first_time_kinematic;
-	bool first_integration;
-	void _update_inertia();
+	PhysicsServer2D::CCDMode continuous_cd_mode = PhysicsServer2D::CCD_MODE_DISABLED;
+	bool omit_force_integration = false;
+	bool active = true;
+	bool can_sleep = true;
+	bool first_time_kinematic = false;
+	void _mass_properties_changed();
 	virtual void _shapes_changed();
 	Transform2D new_transform;
 
@@ -118,7 +122,7 @@ class Body2DSW : public CollisionObject2DSW {
 	};
 
 	Vector<Contact> contacts; //no contacts by default
-	int contact_count;
+	int contact_count = 0;
 
 	void *body_state_callback_instance = nullptr;
 	PhysicsServer2D::BodyStateCallback body_state_callback = nullptr;
@@ -132,7 +136,7 @@ class Body2DSW : public CollisionObject2DSW {
 
 	PhysicsDirectBodyState2DSW *direct_state = nullptr;
 
-	uint64_t island_step;
+	uint64_t island_step = 0;
 
 	_FORCE_INLINE_ void _compute_area_gravity_and_damping(const Area2DSW *p_area);
 
@@ -210,7 +214,7 @@ public:
 
 	_FORCE_INLINE_ void apply_impulse(const Vector2 &p_impulse, const Vector2 &p_position = Vector2()) {
 		linear_velocity += p_impulse * _inv_mass;
-		angular_velocity += _inv_inertia * p_position.cross(p_impulse);
+		angular_velocity += _inv_inertia * (p_position - center_of_mass).cross(p_impulse);
 	}
 
 	_FORCE_INLINE_ void apply_torque_impulse(real_t p_torque) {
@@ -219,7 +223,7 @@ public:
 
 	_FORCE_INLINE_ void apply_bias_impulse(const Vector2 &p_impulse, const Vector2 &p_position = Vector2()) {
 		biased_linear_velocity += p_impulse * _inv_mass;
-		biased_angular_velocity += _inv_inertia * p_position.cross(p_impulse);
+		biased_angular_velocity += _inv_inertia * (p_position - center_of_mass).cross(p_impulse);
 	}
 
 	void set_active(bool p_active);
@@ -232,8 +236,8 @@ public:
 		set_active(true);
 	}
 
-	void set_param(PhysicsServer2D::BodyParameter p_param, real_t);
-	real_t get_param(PhysicsServer2D::BodyParameter p_param) const;
+	void set_param(PhysicsServer2D::BodyParameter p_param, const Variant &p_value);
+	Variant get_param(PhysicsServer2D::BodyParameter p_param) const;
 
 	void set_mode(PhysicsServer2D::BodyMode p_mode);
 	PhysicsServer2D::BodyMode get_mode() const;
@@ -253,7 +257,7 @@ public:
 
 	_FORCE_INLINE_ void add_force(const Vector2 &p_force, const Vector2 &p_position = Vector2()) {
 		applied_force += p_force;
-		applied_torque += p_position.cross(p_force);
+		applied_torque += (p_position - center_of_mass).cross(p_force);
 	}
 
 	_FORCE_INLINE_ void add_torque(real_t p_torque) {
@@ -265,8 +269,10 @@ public:
 
 	void set_space(Space2DSW *p_space);
 
-	void update_inertias();
+	void update_mass_properties();
+	void reset_mass_properties();
 
+	_FORCE_INLINE_ Vector2 get_center_of_mass() const { return center_of_mass; }
 	_FORCE_INLINE_ real_t get_inv_mass() const { return _inv_mass; }
 	_FORCE_INLINE_ real_t get_inv_inertia() const { return _inv_inertia; }
 	_FORCE_INLINE_ real_t get_friction() const { return friction; }

servers/physics_2d/body_direct_state_2d_sw.cpp

@@ -46,6 +46,10 @@ real_t PhysicsDirectBodyState2DSW::get_total_linear_damp() const {
 	return body->area_linear_damp;
 }
 
+Vector2 PhysicsDirectBodyState2DSW::get_center_of_mass() const {
+	return body->get_center_of_mass();
+}
+
 real_t PhysicsDirectBodyState2DSW::get_inverse_mass() const {
 	return body->get_inv_mass();
 }

servers/physics_2d/body_direct_state_2d_sw.h

@@ -45,6 +45,7 @@ public:
 	virtual real_t get_total_angular_damp() const override;
 	virtual real_t get_total_linear_damp() const override;
 
+	virtual Vector2 get_center_of_mass() const override;
 	virtual real_t get_inverse_mass() const override;
 	virtual real_t get_inverse_inertia() const override;
 

servers/physics_2d/joints_2d_sw.cpp

@@ -68,13 +68,13 @@ static inline real_t k_scalar(Body2DSW *a, Body2DSW *b, const Vector2 &rA, const
 
 	{
 		value += a->get_inv_mass();
-		real_t rcn = rA.cross(n);
+		real_t rcn = (rA - a->get_center_of_mass()).cross(n);
 		value += a->get_inv_inertia() * rcn * rcn;
 	}
 
 	if (b) {
 		value += b->get_inv_mass();
-		real_t rcn = rB.cross(n);
+		real_t rcn = (rB - b->get_center_of_mass()).cross(n);
 		value += b->get_inv_inertia() * rcn * rcn;
 	}
 
@@ -83,9 +83,9 @@ static inline real_t k_scalar(Body2DSW *a, Body2DSW *b, const Vector2 &rA, const
 
 static inline Vector2
 relative_velocity(Body2DSW *a, Body2DSW *b, Vector2 rA, Vector2 rB) {
-	Vector2 sum = a->get_linear_velocity() - rA.orthogonal() * a->get_angular_velocity();
+	Vector2 sum = a->get_linear_velocity() - (rA - a->get_center_of_mass()).orthogonal() * a->get_angular_velocity();
 	if (b) {
-		return (b->get_linear_velocity() - rB.orthogonal() * b->get_angular_velocity()) - sum;
+		return (b->get_linear_velocity() - (rB - b->get_center_of_mass()).orthogonal() * b->get_angular_velocity()) - sum;
 	} else {
 		return -sum;
 	}
@@ -172,11 +172,11 @@ bool PinJoint2DSW::pre_solve(real_t p_step) {
 
 void PinJoint2DSW::solve(real_t p_step) {
 	// compute relative velocity
-	Vector2 vA = A->get_linear_velocity() - custom_cross(rA, A->get_angular_velocity());
+	Vector2 vA = A->get_linear_velocity() - custom_cross(rA - A->get_center_of_mass(), A->get_angular_velocity());
 
 	Vector2 rel_vel;
 	if (B) {
-		rel_vel = B->get_linear_velocity() - custom_cross(rB, B->get_angular_velocity()) - vA;
+		rel_vel = B->get_linear_velocity() - custom_cross(rB - B->get_center_of_mass(), B->get_angular_velocity()) - vA;
 	} else {
 		rel_vel = -vA;
 	}
@@ -238,6 +238,9 @@ k_tensor(Body2DSW *a, Body2DSW *b, Vector2 r1, Vector2 r2, Vector2 *k1, Vector2
 	k21 = 0.0f;
 	k22 = m_sum;
 
+	r1 -= a->get_center_of_mass();
+	r2 -= b->get_center_of_mass();
+
 	// add the influence from r1
 	real_t a_i_inv = a->get_inv_inertia();
 	real_t r1xsq = r1.x * r1.x * a_i_inv;

servers/physics_2d/physics_server_2d_sw.cpp

@@ -743,20 +743,27 @@ uint32_t PhysicsServer2DSW::body_get_collision_mask(RID p_body) const {
 	return body->get_collision_mask();
 };
 
-void PhysicsServer2DSW::body_set_param(RID p_body, BodyParameter p_param, real_t p_value) {
+void PhysicsServer2DSW::body_set_param(RID p_body, BodyParameter p_param, const Variant &p_value) {
 	Body2DSW *body = body_owner.getornull(p_body);
 	ERR_FAIL_COND(!body);
 
 	body->set_param(p_param, p_value);
 };
 
-real_t PhysicsServer2DSW::body_get_param(RID p_body, BodyParameter p_param) const {
+Variant PhysicsServer2DSW::body_get_param(RID p_body, BodyParameter p_param) const {
 	Body2DSW *body = body_owner.getornull(p_body);
 	ERR_FAIL_COND_V(!body, 0);
 
 	return body->get_param(p_param);
 };
 
+void PhysicsServer2DSW::body_reset_mass_properties(RID p_body) {
+	Body2DSW *body = body_owner.getornull(p_body);
+	ERR_FAIL_COND(!body);
+
+	return body->reset_mass_properties();
+}
+
 void PhysicsServer2DSW::body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) {
 	Body2DSW *body = body_owner.getornull(p_body);
 	ERR_FAIL_COND(!body);

servers/physics_2d/physics_server_2d_sw.h

@@ -205,8 +205,10 @@ public:
 	virtual void body_set_collision_mask(RID p_body, uint32_t p_mask) override;
 	virtual uint32_t body_get_collision_mask(RID p_body) const override;
 
-	virtual void body_set_param(RID p_body, BodyParameter p_param, real_t p_value) override;
-	virtual real_t body_get_param(RID p_body, BodyParameter p_param) const override;
+	virtual void body_set_param(RID p_body, BodyParameter p_param, const Variant &p_value) override;
+	virtual Variant body_get_param(RID p_body, BodyParameter p_param) const override;
+
+	virtual void body_reset_mass_properties(RID p_body) override;
 
 	virtual void body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) override;
 	virtual Variant body_get_state(RID p_body, BodyState p_state) const override;

servers/physics_2d/physics_server_2d_wrap_mt.h

@@ -212,8 +212,10 @@ public:
 	FUNC2(body_set_collision_mask, RID, uint32_t);
 	FUNC1RC(uint32_t, body_get_collision_mask, RID);
 
-	FUNC3(body_set_param, RID, BodyParameter, real_t);
-	FUNC2RC(real_t, body_get_param, RID, BodyParameter);
+	FUNC3(body_set_param, RID, BodyParameter, const Variant &);
+	FUNC2RC(Variant, body_get_param, RID, BodyParameter);
+
+	FUNC1(body_reset_mass_properties, RID);
 
 	FUNC3(body_set_state, RID, BodyState, const Variant &);
 	FUNC2RC(Variant, body_get_state, RID, BodyState);

servers/physics_2d/space_2d_sw.cpp

@@ -482,7 +482,7 @@ bool PhysicsDirectSpaceState2DSW::rest_info(RID p_shape, const Transform2D &p_sh
 	r_info->metadata = rcd.best_object->get_shape_metadata(rcd.best_shape);
 	if (rcd.best_object->get_type() == CollisionObject2DSW::TYPE_BODY) {
 		const Body2DSW *body = static_cast<const Body2DSW *>(rcd.best_object);
-		Vector2 rel_vec = r_info->point - body->get_transform().get_origin();
+		Vector2 rel_vec = r_info->point - (body->get_transform().get_origin() + body->get_center_of_mass());
 		r_info->linear_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity();
 
 	} else {
@@ -961,7 +961,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
 				r_result->collider_metadata = rcd.best_object->get_shape_metadata(rcd.best_shape);
 
 				const Body2DSW *body = static_cast<const Body2DSW *>(rcd.best_object);
-				Vector2 rel_vec = r_result->collision_point - body->get_transform().get_origin();
+				Vector2 rel_vec = r_result->collision_point - (body->get_transform().get_origin() + body->get_center_of_mass());
 				r_result->collider_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity();
 
 				r_result->travel = safe * p_motion;
@@ -1041,12 +1041,12 @@ void Space2DSW::body_remove_from_active_list(SelfList<Body2DSW> *p_body) {
 	active_list.remove(p_body);
 }
 
-void Space2DSW::body_add_to_inertia_update_list(SelfList<Body2DSW> *p_body) {
-	inertia_update_list.add(p_body);
+void Space2DSW::body_add_to_mass_properties_update_list(SelfList<Body2DSW> *p_body) {
+	mass_properties_update_list.add(p_body);
 }
 
-void Space2DSW::body_remove_from_inertia_update_list(SelfList<Body2DSW> *p_body) {
-	inertia_update_list.remove(p_body);
+void Space2DSW::body_remove_from_mass_properties_update_list(SelfList<Body2DSW> *p_body) {
+	mass_properties_update_list.remove(p_body);
 }
 
 BroadPhase2DSW *Space2DSW::get_broadphase() {
@@ -1112,9 +1112,9 @@ void Space2DSW::call_queries() {
 void Space2DSW::setup() {
 	contact_debug_count = 0;
 
-	while (inertia_update_list.first()) {
-		inertia_update_list.first()->self()->update_inertias();
-		inertia_update_list.remove(inertia_update_list.first());
+	while (mass_properties_update_list.first()) {
+		mass_properties_update_list.first()->self()->update_mass_properties();
+		mass_properties_update_list.remove(mass_properties_update_list.first());
 	}
 }
 

servers/physics_2d/space_2d_sw.h

@@ -86,7 +86,7 @@ private:
 
 	BroadPhase2DSW *broadphase;
 	SelfList<Body2DSW>::List active_list;
-	SelfList<Body2DSW>::List inertia_update_list;
+	SelfList<Body2DSW>::List mass_properties_update_list;
 	SelfList<Body2DSW>::List state_query_list;
 	SelfList<Area2DSW>::List monitor_query_list;
 	SelfList<Area2DSW>::List area_moved_list;
@@ -140,8 +140,8 @@ public:
 	const SelfList<Body2DSW>::List &get_active_body_list() const;
 	void body_add_to_active_list(SelfList<Body2DSW> *p_body);
 	void body_remove_from_active_list(SelfList<Body2DSW> *p_body);
-	void body_add_to_inertia_update_list(SelfList<Body2DSW> *p_body);
-	void body_remove_from_inertia_update_list(SelfList<Body2DSW> *p_body);
+	void body_add_to_mass_properties_update_list(SelfList<Body2DSW> *p_body);
+	void body_remove_from_mass_properties_update_list(SelfList<Body2DSW> *p_body);
 	void area_add_to_moved_list(SelfList<Area2DSW> *p_area);
 	void area_remove_from_moved_list(SelfList<Area2DSW> *p_area);
 	const SelfList<Area2DSW>::List &get_moved_area_list() const;

servers/physics_3d/body_3d_sw.cpp

@@ -34,9 +34,9 @@
 #include "body_direct_state_3d_sw.h"
 #include "space_3d_sw.h"
 
-void Body3DSW::_update_inertia() {
-	if (get_space() && !inertia_update_list.in_list()) {
-		get_space()->body_add_to_inertia_update_list(&inertia_update_list);
+void Body3DSW::_mass_properties_changed() {
+	if (get_space() && !mass_properties_update_list.in_list() && (calculate_inertia || calculate_center_of_mass)) {
+		get_space()->body_add_to_mass_properties_update_list(&mass_properties_update_list);
 	}
 }
 
@@ -44,7 +44,7 @@ void Body3DSW::_update_transform_dependant() {
 	center_of_mass = get_transform().basis.xform(center_of_mass_local);
 	principal_inertia_axes = get_transform().basis * principal_inertia_axes_local;
 
-	// update inertia tensor
+	// Update inertia tensor.
 	Basis tb = principal_inertia_axes;
 	Basis tbt = tb.transposed();
 	Basis diag;
@@ -52,74 +52,95 @@ void Body3DSW::_update_transform_dependant() {
 	_inv_inertia_tensor = tb * diag * tbt;
 }
 
-void Body3DSW::update_inertias() {
+void Body3DSW::update_mass_properties() {
 	// Update shapes and motions.
 
 	switch (mode) {
 		case PhysicsServer3D::BODY_MODE_DYNAMIC: {
-			// Update tensor for all shapes, not the best way but should be somehow OK. (inspired from bullet)
 			real_t total_area = 0;
-
 			for (int i = 0; i < get_shape_count(); i++) {
+				if (is_shape_disabled(i)) {
+					continue;
+				}
+
 				total_area += get_shape_area(i);
 			}
 
-			// We have to recompute the center of mass.
-			center_of_mass_local.zero();
+			if (calculate_center_of_mass) {
+				// We have to recompute the center of mass.
+				center_of_mass_local.zero();
 
-			if (total_area != 0.0) {
-				for (int i = 0; i < get_shape_count(); i++) {
-					real_t area = get_shape_area(i);
+				if (total_area != 0.0) {
+					for (int i = 0; i < get_shape_count(); i++) {
+						if (is_shape_disabled(i)) {
+							continue;
+						}
 
-					real_t mass = area * this->mass / total_area;
+						real_t area = get_shape_area(i);
 
-					// NOTE: we assume that the shape origin is also its center of mass.
-					center_of_mass_local += mass * get_shape_transform(i).origin;
-				}
+						real_t mass = area * this->mass / total_area;
+
+						// NOTE: we assume that the shape origin is also its center of mass.
+						center_of_mass_local += mass * get_shape_transform(i).origin;
+					}
 
-				center_of_mass_local /= mass;
+					center_of_mass_local /= mass;
+				}
 			}
 
-			// Recompute the inertia tensor.
-			Basis inertia_tensor;
-			inertia_tensor.set_zero();
-			bool inertia_set = false;
+			if (calculate_inertia) {
+				// Recompute the inertia tensor.
+				Basis inertia_tensor;
+				inertia_tensor.set_zero();
+				bool inertia_set = false;
 
-			for (int i = 0; i < get_shape_count(); i++) {
-				if (is_shape_disabled(i)) {
-					continue;
-				}
+				for (int i = 0; i < get_shape_count(); i++) {
+					if (is_shape_disabled(i)) {
+						continue;
+					}
 
-				real_t area = get_shape_area(i);
-				if (area == 0.0) {
-					continue;
-				}
+					real_t area = get_shape_area(i);
+					if (area == 0.0) {
+						continue;
+					}
 
-				inertia_set = true;
+					inertia_set = true;
 
-				const Shape3DSW *shape = get_shape(i);
+					const Shape3DSW *shape = get_shape(i);
 
-				real_t mass = area * this->mass / total_area;
+					real_t mass = area * this->mass / total_area;
 
-				Basis shape_inertia_tensor = shape->get_moment_of_inertia(mass).to_diagonal_matrix();
-				Transform3D shape_transform = get_shape_transform(i);
-				Basis shape_basis = shape_transform.basis.orthonormalized();
+					Basis shape_inertia_tensor = shape->get_moment_of_inertia(mass).to_diagonal_matrix();
+					Transform3D shape_transform = get_shape_transform(i);
+					Basis shape_basis = shape_transform.basis.orthonormalized();
 
-				// NOTE: we don't take the scale of collision shapes into account when computing the inertia tensor!
-				shape_inertia_tensor = shape_basis * shape_inertia_tensor * shape_basis.transposed();
+					// NOTE: we don't take the scale of collision shapes into account when computing the inertia tensor!
+					shape_inertia_tensor = shape_basis * shape_inertia_tensor * shape_basis.transposed();
 
-				Vector3 shape_origin = shape_transform.origin - center_of_mass_local;
-				inertia_tensor += shape_inertia_tensor + (Basis() * shape_origin.dot(shape_origin) - shape_origin.outer(shape_origin)) * mass;
-			}
+					Vector3 shape_origin = shape_transform.origin - center_of_mass_local;
+					inertia_tensor += shape_inertia_tensor + (Basis() * shape_origin.dot(shape_origin) - shape_origin.outer(shape_origin)) * mass;
+				}
 
-			// Set the inertia to a valid value when there are no valid shapes.
-			if (!inertia_set) {
-				inertia_tensor.set_diagonal(Vector3(1.0, 1.0, 1.0));
-			}
+				// Set the inertia to a valid value when there are no valid shapes.
+				if (!inertia_set) {
+					inertia_tensor.set_diagonal(Vector3(1.0, 1.0, 1.0));
+				}
 
-			// Compute the principal axes of inertia.
-			principal_inertia_axes_local = inertia_tensor.diagonalize().transposed();
-			_inv_inertia = inertia_tensor.get_main_diagonal().inverse();
+				// Handle partial custom inertia.
+				if (inertia.x > 0.0) {
+					inertia_tensor[0][0] = inertia.x;
+				}
+				if (inertia.y > 0.0) {
+					inertia_tensor[1][1] = inertia.y;
+				}
+				if (inertia.z > 0.0) {
+					inertia_tensor[2][2] = inertia.z;
+				}
+
+				// Compute the principal axes of inertia.
+				principal_inertia_axes_local = inertia_tensor.diagonalize().transposed();
+				_inv_inertia = inertia_tensor.get_main_diagonal().inverse();
+			}
 
 			if (mass) {
 				_inv_mass = 1.0 / mass;
@@ -128,10 +149,9 @@ void Body3DSW::update_inertias() {
 			}
 
 		} break;
-
 		case PhysicsServer3D::BODY_MODE_KINEMATIC:
 		case PhysicsServer3D::BODY_MODE_STATIC: {
-			_inv_inertia_tensor.set_zero();
+			_inv_inertia = Vector3();
 			_inv_mass = 0;
 		} break;
 		case PhysicsServer3D::BODY_MODE_DYNAMIC_LOCKED: {
@@ -141,11 +161,15 @@ void Body3DSW::update_inertias() {
 		} break;
 	}
 
-	//_update_shapes();
-
 	_update_transform_dependant();
 }
 
+void Body3DSW::reset_mass_properties() {
+	calculate_inertia = true;
+	calculate_center_of_mass = true;
+	_mass_properties_changed();
+}
+
 void Body3DSW::set_active(bool p_active) {
 	if (active == p_active) {
 		return;
@@ -165,7 +189,7 @@ void Body3DSW::set_active(bool p_active) {
 	}
 }
 
-void Body3DSW::set_param(PhysicsServer3D::BodyParameter p_param, real_t p_value) {
+void Body3DSW::set_param(PhysicsServer3D::BodyParameter p_param, const Variant &p_value) {
 	switch (p_param) {
 		case PhysicsServer3D::BODY_PARAM_BOUNCE: {
 			bounce = p_value;
@@ -174,10 +198,33 @@ void Body3DSW::set_param(PhysicsServer3D::BodyParameter p_param, real_t p_value)
 			friction = p_value;
 		} break;
 		case PhysicsServer3D::BODY_PARAM_MASS: {
-			ERR_FAIL_COND(p_value <= 0);
-			mass = p_value;
-			_update_inertia();
-
+			real_t mass_value = p_value;
+			ERR_FAIL_COND(mass_value <= 0);
+			mass = mass_value;
+			if (mode >= PhysicsServer3D::BODY_MODE_DYNAMIC) {
+				_mass_properties_changed();
+			}
+		} break;
+		case PhysicsServer3D::BODY_PARAM_INERTIA: {
+			inertia = p_value;
+			if ((inertia.x <= 0.0) || (inertia.y <= 0.0) || (inertia.z <= 0.0)) {
+				calculate_inertia = true;
+				if (mode == PhysicsServer3D::BODY_MODE_DYNAMIC) {
+					_mass_properties_changed();
+				}
+			} else {
+				calculate_inertia = false;
+				if (mode == PhysicsServer3D::BODY_MODE_DYNAMIC) {
+					principal_inertia_axes_local.set_diagonal(Vector3(1.0, 1.0, 1.0));
+					_inv_inertia = inertia.inverse();
+					_update_transform_dependant();
+				}
+			}
+		} break;
+		case PhysicsServer3D::BODY_PARAM_CENTER_OF_MASS: {
+			calculate_center_of_mass = false;
+			center_of_mass_local = p_value;
+			_update_transform_dependant();
 		} break;
 		case PhysicsServer3D::BODY_PARAM_GRAVITY_SCALE: {
 			gravity_scale = p_value;
@@ -193,7 +240,7 @@ void Body3DSW::set_param(PhysicsServer3D::BodyParameter p_param, real_t p_value)
 	}
 }
 
-real_t Body3DSW::get_param(PhysicsServer3D::BodyParameter p_param) const {
+Variant Body3DSW::get_param(PhysicsServer3D::BodyParameter p_param) const {
 	switch (p_param) {
 		case PhysicsServer3D::BODY_PARAM_BOUNCE: {
 			return bounce;
@@ -204,6 +251,16 @@ real_t Body3DSW::get_param(PhysicsServer3D::BodyParameter p_param) const {
 		case PhysicsServer3D::BODY_PARAM_MASS: {
 			return mass;
 		} break;
+		case PhysicsServer3D::BODY_PARAM_INERTIA: {
+			if (mode == PhysicsServer3D::BODY_MODE_DYNAMIC) {
+				return _inv_inertia.inverse();
+			} else {
+				return Vector3();
+			}
+		} break;
+		case PhysicsServer3D::BODY_PARAM_CENTER_OF_MASS: {
+			return center_of_mass;
+		} break;
 		case PhysicsServer3D::BODY_PARAM_GRAVITY_SCALE: {
 			return gravity_scale;
 		} break;
@@ -226,40 +283,42 @@ void Body3DSW::set_mode(PhysicsServer3D::BodyMode p_mode) {
 	mode = p_mode;
 
 	switch (p_mode) {
-		//CLEAR UP EVERYTHING IN CASE IT NOT WORKS!
 		case PhysicsServer3D::BODY_MODE_STATIC:
 		case PhysicsServer3D::BODY_MODE_KINEMATIC: {
 			_set_inv_transform(get_transform().affine_inverse());
 			_inv_mass = 0;
+			_inv_inertia = Vector3();
 			_set_static(p_mode == PhysicsServer3D::BODY_MODE_STATIC);
-			//set_active(p_mode==PhysicsServer3D::BODY_MODE_KINEMATIC);
 			set_active(p_mode == PhysicsServer3D::BODY_MODE_KINEMATIC && contacts.size());
 			linear_velocity = Vector3();
 			angular_velocity = Vector3();
 			if (mode == PhysicsServer3D::BODY_MODE_KINEMATIC && prev != mode) {
 				first_time_kinematic = true;
 			}
+			_update_transform_dependant();
 
 		} break;
 		case PhysicsServer3D::BODY_MODE_DYNAMIC: {
 			_inv_mass = mass > 0 ? (1.0 / mass) : 0;
+			if (!calculate_inertia) {
+				principal_inertia_axes_local.set_diagonal(Vector3(1.0, 1.0, 1.0));
+				_inv_inertia = inertia.inverse();
+				_update_transform_dependant();
+			}
+			_mass_properties_changed();
 			_set_static(false);
 			set_active(true);
 
 		} break;
 		case PhysicsServer3D::BODY_MODE_DYNAMIC_LOCKED: {
 			_inv_mass = mass > 0 ? (1.0 / mass) : 0;
+			_inv_inertia = Vector3();
+			angular_velocity = Vector3();
+			_update_transform_dependant();
 			_set_static(false);
 			set_active(true);
-			angular_velocity = Vector3();
-		} break;
+		}
 	}
-
-	_update_inertia();
-	/*
-	if (get_space())
-		_update_queries();
-	*/
 }
 
 PhysicsServer3D::BodyMode Body3DSW::get_mode() const {
@@ -267,7 +326,7 @@ PhysicsServer3D::BodyMode Body3DSW::get_mode() const {
 }
 
 void Body3DSW::_shapes_changed() {
-	_update_inertia();
+	_mass_properties_changed();
 }
 
 void Body3DSW::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_variant) {
@@ -328,7 +387,7 @@ void Body3DSW::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_va
 		} break;
 		case PhysicsServer3D::BODY_STATE_CAN_SLEEP: {
 			can_sleep = p_variant;
-			if (mode == PhysicsServer3D::BODY_MODE_DYNAMIC && !active && !can_sleep) {
+			if (mode >= PhysicsServer3D::BODY_MODE_DYNAMIC && !active && !can_sleep) {
 				set_active(true);
 			}
 
@@ -360,8 +419,8 @@ Variant Body3DSW::get_state(PhysicsServer3D::BodyState p_state) const {
 
 void Body3DSW::set_space(Space3DSW *p_space) {
 	if (get_space()) {
-		if (inertia_update_list.in_list()) {
-			get_space()->body_remove_from_inertia_update_list(&inertia_update_list);
+		if (mass_properties_update_list.in_list()) {
+			get_space()->body_remove_from_mass_properties_update_list(&mass_properties_update_list);
 		}
 		if (active_list.in_list()) {
 			get_space()->body_remove_from_active_list(&active_list);
@@ -374,13 +433,11 @@ void Body3DSW::set_space(Space3DSW *p_space) {
 	_set_space(p_space);
 
 	if (get_space()) {
-		_update_inertia();
+		_mass_properties_changed();
 		if (active) {
 			get_space()->body_add_to_active_list(&active_list);
 		}
 	}
-
-	first_integration = true;
 }
 
 void Body3DSW::_compute_area_gravity_and_damping(const Area3DSW *p_area) {
@@ -486,7 +543,7 @@ void Body3DSW::integrate_forces(real_t p_step) {
 		axis.normalize();
 		angular_velocity = constant_angular_velocity + axis * (angle / p_step);
 	} else {
-		if (!omit_force_integration && !first_integration) {
+		if (!omit_force_integration) {
 			//overridden by direct state query
 
 			Vector3 force = gravity * mass;
@@ -520,7 +577,6 @@ void Body3DSW::integrate_forces(real_t p_step) {
 
 	applied_force = Vector3();
 	applied_torque = Vector3();
-	first_integration = false;
 
 	//motion=linear_velocity*p_step;
 
@@ -642,7 +698,7 @@ void Body3DSW::wakeup_neighbours() {
 				continue;
 			}
 			Body3DSW *b = n[i];
-			if (b->mode != PhysicsServer3D::BODY_MODE_DYNAMIC) {
+			if (b->mode < PhysicsServer3D::BODY_MODE_DYNAMIC) {
 				continue;
 			}
 
@@ -719,32 +775,9 @@ PhysicsDirectBodyState3DSW *Body3DSW::get_direct_state() {
 Body3DSW::Body3DSW() :
 		CollisionObject3DSW(TYPE_BODY),
 		active_list(this),
-		inertia_update_list(this),
+		mass_properties_update_list(this),
 		direct_state_query_list(this) {
-	mode = PhysicsServer3D::BODY_MODE_DYNAMIC;
-	active = true;
-
-	mass = 1;
-	_inv_mass = 1;
-	bounce = 0;
-	friction = 1;
-	omit_force_integration = false;
-	//applied_torque=0;
-	island_step = 0;
-	first_time_kinematic = false;
-	first_integration = false;
 	_set_static(false);
-
-	contact_count = 0;
-	gravity_scale = 1.0;
-	linear_damp = -1;
-	angular_damp = -1;
-	area_angular_damp = 0;
-	area_linear_damp = 0;
-
-	still_time = 0;
-	continuous_cd = false;
-	can_sleep = true;
 }
 
 Body3DSW::~Body3DSW() {

servers/physics_3d/body_3d_sw.h

@@ -39,7 +39,7 @@ class Constraint3DSW;
 class PhysicsDirectBodyState3DSW;
 
 class Body3DSW : public CollisionObject3DSW {
-	PhysicsServer3D::BodyMode mode;
+	PhysicsServer3D::BodyMode mode = PhysicsServer3D::BODY_MODE_DYNAMIC;
 
 	Vector3 linear_velocity;
 	Vector3 angular_velocity;
@@ -49,17 +49,18 @@ class Body3DSW : public CollisionObject3DSW {
 
 	Vector3 biased_linear_velocity;
 	Vector3 biased_angular_velocity;
-	real_t mass;
-	real_t bounce;
-	real_t friction;
+	real_t mass = 1.0;
+	real_t bounce = 0.0;
+	real_t friction = 1.0;
+	Vector3 inertia;
 
-	real_t linear_damp;
-	real_t angular_damp;
-	real_t gravity_scale;
+	real_t linear_damp = -1.0;
+	real_t angular_damp = -1.0;
+	real_t gravity_scale = 1.0;
 
 	uint16_t locked_axis = 0;
 
-	real_t _inv_mass;
+	real_t _inv_mass = 1.0;
 	Vector3 _inv_inertia; // Relative to the principal axes of inertia
 
 	// Relative to the local frame of reference
@@ -71,30 +72,32 @@ class Body3DSW : public CollisionObject3DSW {
 	Basis principal_inertia_axes;
 	Vector3 center_of_mass;
 
+	bool calculate_inertia = true;
+	bool calculate_center_of_mass = true;
+
 	Vector3 gravity;
 
-	real_t still_time;
+	real_t still_time = 0.0;
 
 	Vector3 applied_force;
 	Vector3 applied_torque;
 
-	real_t area_angular_damp;
-	real_t area_linear_damp;
+	real_t area_angular_damp = 0.0;
+	real_t area_linear_damp = 0.0;
 
 	SelfList<Body3DSW> active_list;
-	SelfList<Body3DSW> inertia_update_list;
+	SelfList<Body3DSW> mass_properties_update_list;
 	SelfList<Body3DSW> direct_state_query_list;
 
 	VSet<RID> exceptions;
-	bool omit_force_integration;
-	bool active;
+	bool omit_force_integration = false;
+	bool active = true;
 
-	bool first_integration;
+	bool continuous_cd = false;
+	bool can_sleep = true;
+	bool first_time_kinematic = false;
 
-	bool continuous_cd;
-	bool can_sleep;
-	bool first_time_kinematic;
-	void _update_inertia();
+	void _mass_properties_changed();
 	virtual void _shapes_changed();
 	Transform3D new_transform;
 
@@ -115,7 +118,7 @@ class Body3DSW : public CollisionObject3DSW {
 	};
 
 	Vector<Contact> contacts; //no contacts by default
-	int contact_count;
+	int contact_count = 0;
 
 	void *body_state_callback_instance = nullptr;
 	PhysicsServer3D::BodyStateCallback body_state_callback = nullptr;
@@ -129,7 +132,7 @@ class Body3DSW : public CollisionObject3DSW {
 
 	PhysicsDirectBodyState3DSW *direct_state = nullptr;
 
-	uint64_t island_step;
+	uint64_t island_step = 0;
 
 	_FORCE_INLINE_ void _compute_area_gravity_and_damping(const Area3DSW *p_area);
 
@@ -254,8 +257,8 @@ public:
 		set_active(true);
 	}
 
-	void set_param(PhysicsServer3D::BodyParameter p_param, real_t);
-	real_t get_param(PhysicsServer3D::BodyParameter p_param) const;
+	void set_param(PhysicsServer3D::BodyParameter p_param, const Variant &p_value);
+	Variant get_param(PhysicsServer3D::BodyParameter p_param) const;
 
 	void set_mode(PhysicsServer3D::BodyMode p_mode);
 	PhysicsServer3D::BodyMode get_mode() const;
@@ -274,7 +277,8 @@ public:
 
 	void set_space(Space3DSW *p_space);
 
-	void update_inertias();
+	void update_mass_properties();
+	void reset_mass_properties();
 
 	_FORCE_INLINE_ real_t get_inv_mass() const { return _inv_mass; }
 	_FORCE_INLINE_ const Vector3 &get_inv_inertia() const { return _inv_inertia; }

servers/physics_3d/physics_server_3d_sw.cpp

@@ -643,20 +643,27 @@ uint32_t PhysicsServer3DSW::body_get_user_flags(RID p_body) const {
 	return 0;
 };
 
-void PhysicsServer3DSW::body_set_param(RID p_body, BodyParameter p_param, real_t p_value) {
+void PhysicsServer3DSW::body_set_param(RID p_body, BodyParameter p_param, const Variant &p_value) {
 	Body3DSW *body = body_owner.getornull(p_body);
 	ERR_FAIL_COND(!body);
 
 	body->set_param(p_param, p_value);
 };
 
-real_t PhysicsServer3DSW::body_get_param(RID p_body, BodyParameter p_param) const {
+Variant PhysicsServer3DSW::body_get_param(RID p_body, BodyParameter p_param) const {
 	Body3DSW *body = body_owner.getornull(p_body);
 	ERR_FAIL_COND_V(!body, 0);
 
 	return body->get_param(p_param);
 };
 
+void PhysicsServer3DSW::body_reset_mass_properties(RID p_body) {
+	Body3DSW *body = body_owner.getornull(p_body);
+	ERR_FAIL_COND(!body);
+
+	return body->reset_mass_properties();
+}
+
 void PhysicsServer3DSW::body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) {
 	Body3DSW *body = body_owner.getornull(p_body);
 	ERR_FAIL_COND(!body);

servers/physics_3d/physics_server_3d_sw.h

@@ -198,8 +198,10 @@ public:
 	virtual void body_set_user_flags(RID p_body, uint32_t p_flags) override;
 	virtual uint32_t body_get_user_flags(RID p_body) const override;
 
-	virtual void body_set_param(RID p_body, BodyParameter p_param, real_t p_value) override;
-	virtual real_t body_get_param(RID p_body, BodyParameter p_param) const override;
+	virtual void body_set_param(RID p_body, BodyParameter p_param, const Variant &p_value) override;
+	virtual Variant body_get_param(RID p_body, BodyParameter p_param) const override;
+
+	virtual void body_reset_mass_properties(RID p_body) override;
 
 	virtual void body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) override;
 	virtual Variant body_get_state(RID p_body, BodyState p_state) const override;

servers/physics_3d/physics_server_3d_wrap_mt.h

@@ -210,8 +210,10 @@ public:
 	FUNC2(body_set_user_flags, RID, uint32_t);
 	FUNC1RC(uint32_t, body_get_user_flags, RID);
 
-	FUNC3(body_set_param, RID, BodyParameter, real_t);
-	FUNC2RC(real_t, body_get_param, RID, BodyParameter);
+	FUNC3(body_set_param, RID, BodyParameter, const Variant &);
+	FUNC2RC(Variant, body_get_param, RID, BodyParameter);
+
+	FUNC1(body_reset_mass_properties, RID);
 
 	FUNC3(body_set_state, RID, BodyState, const Variant &);
 	FUNC2RC(Variant, body_get_state, RID, BodyState);

servers/physics_3d/space_3d_sw.cpp

@@ -977,12 +977,12 @@ void Space3DSW::body_remove_from_active_list(SelfList<Body3DSW> *p_body) {
 	active_list.remove(p_body);
 }
 
-void Space3DSW::body_add_to_inertia_update_list(SelfList<Body3DSW> *p_body) {
-	inertia_update_list.add(p_body);
+void Space3DSW::body_add_to_mass_properties_update_list(SelfList<Body3DSW> *p_body) {
+	mass_properties_update_list.add(p_body);
 }
 
-void Space3DSW::body_remove_from_inertia_update_list(SelfList<Body3DSW> *p_body) {
-	inertia_update_list.remove(p_body);
+void Space3DSW::body_remove_from_mass_properties_update_list(SelfList<Body3DSW> *p_body) {
+	mass_properties_update_list.remove(p_body);
 }
 
 BroadPhase3DSW *Space3DSW::get_broadphase() {
@@ -1059,9 +1059,9 @@ void Space3DSW::call_queries() {
 
 void Space3DSW::setup() {
 	contact_debug_count = 0;
-	while (inertia_update_list.first()) {
-		inertia_update_list.first()->self()->update_inertias();
-		inertia_update_list.remove(inertia_update_list.first());
+	while (mass_properties_update_list.first()) {
+		mass_properties_update_list.first()->self()->update_mass_properties();
+		mass_properties_update_list.remove(mass_properties_update_list.first());
 	}
 }
 

servers/physics_3d/space_3d_sw.h

@@ -79,7 +79,7 @@ private:
 
 	BroadPhase3DSW *broadphase;
 	SelfList<Body3DSW>::List active_list;
-	SelfList<Body3DSW>::List inertia_update_list;
+	SelfList<Body3DSW>::List mass_properties_update_list;
 	SelfList<Body3DSW>::List state_query_list;
 	SelfList<Area3DSW>::List monitor_query_list;
 	SelfList<Area3DSW>::List area_moved_list;
@@ -137,8 +137,8 @@ public:
 	const SelfList<Body3DSW>::List &get_active_body_list() const;
 	void body_add_to_active_list(SelfList<Body3DSW> *p_body);
 	void body_remove_from_active_list(SelfList<Body3DSW> *p_body);
-	void body_add_to_inertia_update_list(SelfList<Body3DSW> *p_body);
-	void body_remove_from_inertia_update_list(SelfList<Body3DSW> *p_body);
+	void body_add_to_mass_properties_update_list(SelfList<Body3DSW> *p_body);
+	void body_remove_from_mass_properties_update_list(SelfList<Body3DSW> *p_body);
 
 	void body_add_to_state_query_list(SelfList<Body3DSW> *p_body);
 	void body_remove_from_state_query_list(SelfList<Body3DSW> *p_body);

servers/physics_server_2d.cpp

@@ -77,6 +77,7 @@ void PhysicsDirectBodyState2D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("get_total_linear_damp"), &PhysicsDirectBodyState2D::get_total_linear_damp);
 	ClassDB::bind_method(D_METHOD("get_total_angular_damp"), &PhysicsDirectBodyState2D::get_total_angular_damp);
 
+	ClassDB::bind_method(D_METHOD("get_center_of_mass"), &PhysicsDirectBodyState2D::get_center_of_mass);
 	ClassDB::bind_method(D_METHOD("get_inverse_mass"), &PhysicsDirectBodyState2D::get_inverse_mass);
 	ClassDB::bind_method(D_METHOD("get_inverse_inertia"), &PhysicsDirectBodyState2D::get_inverse_inertia);
 
@@ -123,6 +124,7 @@ void PhysicsDirectBodyState2D::_bind_methods() {
 	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "total_angular_damp"), "", "get_total_angular_damp");
 	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "total_linear_damp"), "", "get_total_linear_damp");
 	ADD_PROPERTY(PropertyInfo(Variant::VECTOR2, "total_gravity"), "", "get_total_gravity");
+	ADD_PROPERTY(PropertyInfo(Variant::VECTOR2, "center_of_mass"), "", "get_center_of_mass");
 	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "angular_velocity"), "set_angular_velocity", "get_angular_velocity");
 	ADD_PROPERTY(PropertyInfo(Variant::VECTOR2, "linear_velocity"), "set_linear_velocity", "get_linear_velocity");
 	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "sleeping"), "set_sleep_state", "is_sleeping");
@@ -607,6 +609,8 @@ void PhysicsServer2D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("body_set_param", "body", "param", "value"), &PhysicsServer2D::body_set_param);
 	ClassDB::bind_method(D_METHOD("body_get_param", "body", "param"), &PhysicsServer2D::body_get_param);
 
+	ClassDB::bind_method(D_METHOD("body_reset_mass_properties", "body"), &PhysicsServer2D::body_reset_mass_properties);
+
 	ClassDB::bind_method(D_METHOD("body_set_state", "body", "state", "value"), &PhysicsServer2D::body_set_state);
 	ClassDB::bind_method(D_METHOD("body_get_state", "body", "state"), &PhysicsServer2D::body_get_state);
 
@@ -702,6 +706,7 @@ void PhysicsServer2D::_bind_methods() {
 	BIND_ENUM_CONSTANT(BODY_PARAM_FRICTION);
 	BIND_ENUM_CONSTANT(BODY_PARAM_MASS);
 	BIND_ENUM_CONSTANT(BODY_PARAM_INERTIA);
+	BIND_ENUM_CONSTANT(BODY_PARAM_CENTER_OF_MASS);
 	BIND_ENUM_CONSTANT(BODY_PARAM_GRAVITY_SCALE);
 	BIND_ENUM_CONSTANT(BODY_PARAM_LINEAR_DAMP);
 	BIND_ENUM_CONSTANT(BODY_PARAM_ANGULAR_DAMP);

servers/physics_server_2d.h

@@ -48,6 +48,7 @@ public:
 	virtual real_t get_total_linear_damp() const = 0; // get density of this body space/area
 	virtual real_t get_total_angular_damp() const = 0; // get density of this body space/area
 
+	virtual Vector2 get_center_of_mass() const = 0;
 	virtual real_t get_inverse_mass() const = 0; // get the mass
 	virtual real_t get_inverse_inertia() const = 0; // get density of this body space
 
@@ -402,15 +403,18 @@ public:
 		BODY_PARAM_BOUNCE,
 		BODY_PARAM_FRICTION,
 		BODY_PARAM_MASS, ///< unused for static, always infinite
-		BODY_PARAM_INERTIA, // read-only: computed from mass & shapes
+		BODY_PARAM_INERTIA,
+		BODY_PARAM_CENTER_OF_MASS,
 		BODY_PARAM_GRAVITY_SCALE,
 		BODY_PARAM_LINEAR_DAMP,
 		BODY_PARAM_ANGULAR_DAMP,
 		BODY_PARAM_MAX,
 	};
 
-	virtual void body_set_param(RID p_body, BodyParameter p_param, real_t p_value) = 0;
-	virtual real_t body_get_param(RID p_body, BodyParameter p_param) const = 0;
+	virtual void body_set_param(RID p_body, BodyParameter p_param, const Variant &p_value) = 0;
+	virtual Variant body_get_param(RID p_body, BodyParameter p_param) const = 0;
+
+	virtual void body_reset_mass_properties(RID p_body) = 0;
 
 	//state
 	enum BodyState {

servers/physics_server_3d.cpp

@@ -577,6 +577,8 @@ void PhysicsServer3D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("body_set_param", "body", "param", "value"), &PhysicsServer3D::body_set_param);
 	ClassDB::bind_method(D_METHOD("body_get_param", "body", "param"), &PhysicsServer3D::body_get_param);
 
+	ClassDB::bind_method(D_METHOD("body_reset_mass_properties", "body"), &PhysicsServer3D::body_reset_mass_properties);
+
 	ClassDB::bind_method(D_METHOD("body_set_state", "body", "state", "value"), &PhysicsServer3D::body_set_state);
 	ClassDB::bind_method(D_METHOD("body_get_state", "body", "state"), &PhysicsServer3D::body_get_state);
 
@@ -782,6 +784,8 @@ void PhysicsServer3D::_bind_methods() {
 	BIND_ENUM_CONSTANT(BODY_PARAM_BOUNCE);
 	BIND_ENUM_CONSTANT(BODY_PARAM_FRICTION);
 	BIND_ENUM_CONSTANT(BODY_PARAM_MASS);
+	BIND_ENUM_CONSTANT(BODY_PARAM_INERTIA);
+	BIND_ENUM_CONSTANT(BODY_PARAM_CENTER_OF_MASS);
 	BIND_ENUM_CONSTANT(BODY_PARAM_GRAVITY_SCALE);
 	BIND_ENUM_CONSTANT(BODY_PARAM_LINEAR_DAMP);
 	BIND_ENUM_CONSTANT(BODY_PARAM_ANGULAR_DAMP);

servers/physics_server_3d.h

@@ -405,14 +405,18 @@ public:
 		BODY_PARAM_BOUNCE,
 		BODY_PARAM_FRICTION,
 		BODY_PARAM_MASS, ///< unused for static, always infinite
+		BODY_PARAM_INERTIA,
+		BODY_PARAM_CENTER_OF_MASS,
 		BODY_PARAM_GRAVITY_SCALE,
 		BODY_PARAM_LINEAR_DAMP,
 		BODY_PARAM_ANGULAR_DAMP,
 		BODY_PARAM_MAX,
 	};
 
-	virtual void body_set_param(RID p_body, BodyParameter p_param, real_t p_value) = 0;
-	virtual real_t body_get_param(RID p_body, BodyParameter p_param) const = 0;
+	virtual void body_set_param(RID p_body, BodyParameter p_param, const Variant &p_value) = 0;
+	virtual Variant body_get_param(RID p_body, BodyParameter p_param) const = 0;
+
+	virtual void body_reset_mass_properties(RID p_body) = 0;
 
 	//state
 	enum BodyState {