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

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

7 files changed, 189 insertions, 141 deletions

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