Merge pull request #49610 from nekomatata/center-of-mass-calculation

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

21 files changed, 392 insertions, 283 deletions

diff --git a/servers/physics_2d/body_2d_sw.cpp b/servers/physics_2d/body_2d_sw.cpp
index d86943e658..edd769aa9a 100644
--- a/servers/physics_2d/body_2d_sw.cpp
+++ b/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() {
diff --git a/servers/physics_2d/body_2d_sw.h b/servers/physics_2d/body_2d_sw.h
index b83afdbec6..95e89786cd 100644
--- a/servers/physics_2d/body_2d_sw.h
+++ b/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; }
diff --git a/servers/physics_2d/body_direct_state_2d_sw.cpp b/servers/physics_2d/body_direct_state_2d_sw.cpp
index 1f68cca843..58250c3077 100644
--- a/servers/physics_2d/body_direct_state_2d_sw.cpp
+++ b/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();
 }
diff --git a/servers/physics_2d/body_direct_state_2d_sw.h b/servers/physics_2d/body_direct_state_2d_sw.h
index aef9186086..34faa174d8 100644
--- a/servers/physics_2d/body_direct_state_2d_sw.h
+++ b/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;
 
diff --git a/servers/physics_2d/joints_2d_sw.cpp b/servers/physics_2d/joints_2d_sw.cpp
index 5a0a628fbc..fa8499a81d 100644
--- a/servers/physics_2d/joints_2d_sw.cpp
+++ b/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;
diff --git a/servers/physics_2d/physics_server_2d_sw.cpp b/servers/physics_2d/physics_server_2d_sw.cpp
index 85b5e40752..d0a42ca95b 100644
--- a/servers/physics_2d/physics_server_2d_sw.cpp
+++ b/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);
diff --git a/servers/physics_2d/physics_server_2d_sw.h b/servers/physics_2d/physics_server_2d_sw.h
index ef1306cf59..6a2d9e37e0 100644
--- a/servers/physics_2d/physics_server_2d_sw.h
+++ b/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;
diff --git a/servers/physics_2d/physics_server_2d_wrap_mt.h b/servers/physics_2d/physics_server_2d_wrap_mt.h
index 6761e37daa..e65c4f5f3a 100644
--- a/servers/physics_2d/physics_server_2d_wrap_mt.h
+++ b/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);
diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp
index 6c23f49928..7dbd1243cc 100644
--- a/servers/physics_2d/space_2d_sw.cpp
+++ b/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());
 	}
 }
 
diff --git a/servers/physics_2d/space_2d_sw.h b/servers/physics_2d/space_2d_sw.h
index 373234ae1c..ad82a14af5 100644
--- a/servers/physics_2d/space_2d_sw.h
+++ b/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;
diff --git a/servers/physics_3d/body_3d_sw.cpp b/servers/physics_3d/body_3d_sw.cpp
index 6640245d91..41745545d8 100644
--- a/servers/physics_3d/body_3d_sw.cpp
+++ b/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() {
diff --git a/servers/physics_3d/body_3d_sw.h b/servers/physics_3d/body_3d_sw.h
index 38b7c4f94c..8b74c7e5b9 100644
--- a/servers/physics_3d/body_3d_sw.h
+++ b/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; }
diff --git a/servers/physics_3d/physics_server_3d_sw.cpp b/servers/physics_3d/physics_server_3d_sw.cpp
index a214e80c6c..8de54c5c48 100644
--- a/servers/physics_3d/physics_server_3d_sw.cpp
+++ b/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);
diff --git a/servers/physics_3d/physics_server_3d_sw.h b/servers/physics_3d/physics_server_3d_sw.h
index f283f83112..071ad0a694 100644
--- a/servers/physics_3d/physics_server_3d_sw.h
+++ b/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;
diff --git a/servers/physics_3d/physics_server_3d_wrap_mt.h b/servers/physics_3d/physics_server_3d_wrap_mt.h
index 8865bd4bd2..58986969d4 100644
--- a/servers/physics_3d/physics_server_3d_wrap_mt.h
+++ b/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);
diff --git a/servers/physics_3d/space_3d_sw.cpp b/servers/physics_3d/space_3d_sw.cpp
index 37dee436df..3b08184e00 100644
--- a/servers/physics_3d/space_3d_sw.cpp
+++ b/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());
 	}
 }
 
diff --git a/servers/physics_3d/space_3d_sw.h b/servers/physics_3d/space_3d_sw.h
index 7d793749d3..98c335cb80 100644
--- a/servers/physics_3d/space_3d_sw.h
+++ b/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);
diff --git a/servers/physics_server_2d.cpp b/servers/physics_server_2d.cpp
index b11bc27daa..2656ef1d6d 100644
--- a/servers/physics_server_2d.cpp
+++ b/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);
diff --git a/servers/physics_server_2d.h b/servers/physics_server_2d.h
index 20cf4cb31b..1145bb8b91 100644
--- a/servers/physics_server_2d.h
+++ b/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 {
diff --git a/servers/physics_server_3d.cpp b/servers/physics_server_3d.cpp
index d0650396d0..0c487b83ea 100644
--- a/servers/physics_server_3d.cpp
+++ b/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);
diff --git a/servers/physics_server_3d.h b/servers/physics_server_3d.h
index c02a8f42f8..5677604682 100644
--- a/servers/physics_server_3d.h
+++ b/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 {