Merge pull request #54011 from nekomatata/center-of-mass-2d-transform

4 files changed, 26 insertions, 12 deletions

diff --git a/servers/physics_2d/godot_body_2d.cpp b/servers/physics_2d/godot_body_2d.cpp
index b0885a1f7b..68f114a34a 100644
--- a/servers/physics_2d/godot_body_2d.cpp
+++ b/servers/physics_2d/godot_body_2d.cpp
@@ -119,6 +119,8 @@ void GodotBody2D::update_mass_properties() {
 
 		} break;
 	}
+
+	_update_transform_dependent();
 }
 
 void GodotBody2D::reset_mass_properties() {
@@ -179,7 +181,8 @@ void GodotBody2D::set_param(PhysicsServer2D::BodyParameter p_param, const Varian
 		} break;
 		case PhysicsServer2D::BODY_PARAM_CENTER_OF_MASS: {
 			calculate_center_of_mass = false;
-			center_of_mass = p_value;
+			center_of_mass_local = p_value;
+			_update_transform_dependent();
 		} break;
 		case PhysicsServer2D::BODY_PARAM_GRAVITY_SCALE: {
 			gravity_scale = p_value;
@@ -301,6 +304,7 @@ void GodotBody2D::set_state(PhysicsServer2D::BodyState p_state, const Variant &p
 				}
 				_set_transform(t);
 				_set_inv_transform(get_transform().inverse());
+				_update_transform_dependent();
 			}
 			wakeup();
 
@@ -400,6 +404,10 @@ void GodotBody2D::_compute_area_gravity_and_damping(const GodotArea2D *p_area) {
 	area_angular_damp += p_area->get_angular_damp();
 }
 
+void GodotBody2D::_update_transform_dependent() {
+	center_of_mass = get_transform().basis_xform(center_of_mass_local);
+}
+
 void GodotBody2D::integrate_forces(real_t p_step) {
 	if (mode == PhysicsServer2D::BODY_MODE_STATIC) {
 		return;
@@ -568,6 +576,8 @@ void GodotBody2D::integrate_velocities(real_t p_step) {
 	if (continuous_cd_mode != PhysicsServer2D::CCD_MODE_DISABLED) {
 		new_transform = get_transform();
 	}
+
+	_update_transform_dependent();
 }
 
 void GodotBody2D::wakeup_neighbours() {
diff --git a/servers/physics_2d/godot_body_2d.h b/servers/physics_2d/godot_body_2d.h
index c4f3578f1b..5fce362fa7 100644
--- a/servers/physics_2d/godot_body_2d.h
+++ b/servers/physics_2d/godot_body_2d.h
@@ -66,6 +66,7 @@ class GodotBody2D : public GodotCollisionObject2D {
 	real_t inertia = 0.0;
 	real_t _inv_inertia = 0.0;
 
+	Vector2 center_of_mass_local;
 	Vector2 center_of_mass;
 
 	bool calculate_inertia = true;
@@ -139,7 +140,9 @@ class GodotBody2D : public GodotCollisionObject2D {
 
 	uint64_t island_step = 0;
 
-	_FORCE_INLINE_ void _compute_area_gravity_and_damping(const GodotArea2D *p_area);
+	void _compute_area_gravity_and_damping(const GodotArea2D *p_area);
+
+	void _update_transform_dependent();
 
 	friend class GodotPhysicsDirectBodyState2D; // i give up, too many functions to expose
 
diff --git a/servers/physics_3d/godot_body_3d.cpp b/servers/physics_3d/godot_body_3d.cpp
index 6df6a0c45b..0564c4d452 100644
--- a/servers/physics_3d/godot_body_3d.cpp
+++ b/servers/physics_3d/godot_body_3d.cpp
@@ -40,7 +40,7 @@ void GodotBody3D::_mass_properties_changed() {
 	}
 }
 
-void GodotBody3D::_update_transform_dependant() {
+void GodotBody3D::_update_transform_dependent() {
 	center_of_mass = get_transform().basis.xform(center_of_mass_local);
 	principal_inertia_axes = get_transform().basis * principal_inertia_axes_local;
 
@@ -161,7 +161,7 @@ void GodotBody3D::update_mass_properties() {
 		} break;
 	}
 
-	_update_transform_dependant();
+	_update_transform_dependent();
 }
 
 void GodotBody3D::reset_mass_properties() {
@@ -217,14 +217,14 @@ void GodotBody3D::set_param(PhysicsServer3D::BodyParameter p_param, const Varian
 				if (mode == PhysicsServer3D::BODY_MODE_DYNAMIC) {
 					principal_inertia_axes_local = Basis();
 					_inv_inertia = inertia.inverse();
-					_update_transform_dependant();
+					_update_transform_dependent();
 				}
 			}
 		} break;
 		case PhysicsServer3D::BODY_PARAM_CENTER_OF_MASS: {
 			calculate_center_of_mass = false;
 			center_of_mass_local = p_value;
-			_update_transform_dependant();
+			_update_transform_dependent();
 		} break;
 		case PhysicsServer3D::BODY_PARAM_GRAVITY_SCALE: {
 			gravity_scale = p_value;
@@ -295,7 +295,7 @@ void GodotBody3D::set_mode(PhysicsServer3D::BodyMode p_mode) {
 			if (mode == PhysicsServer3D::BODY_MODE_KINEMATIC && prev != mode) {
 				first_time_kinematic = true;
 			}
-			_update_transform_dependant();
+			_update_transform_dependent();
 
 		} break;
 		case PhysicsServer3D::BODY_MODE_DYNAMIC: {
@@ -303,7 +303,7 @@ void GodotBody3D::set_mode(PhysicsServer3D::BodyMode p_mode) {
 			if (!calculate_inertia) {
 				principal_inertia_axes_local = Basis();
 				_inv_inertia = inertia.inverse();
-				_update_transform_dependant();
+				_update_transform_dependent();
 			}
 			_mass_properties_changed();
 			_set_static(false);
@@ -314,7 +314,7 @@ void GodotBody3D::set_mode(PhysicsServer3D::BodyMode p_mode) {
 			_inv_mass = mass > 0 ? (1.0 / mass) : 0;
 			_inv_inertia = Vector3();
 			angular_velocity = Vector3();
-			_update_transform_dependant();
+			_update_transform_dependent();
 			_set_static(false);
 			set_active(true);
 		}
@@ -355,6 +355,7 @@ void GodotBody3D::set_state(PhysicsServer3D::BodyState p_state, const Variant &p
 				}
 				_set_transform(t);
 				_set_inv_transform(get_transform().inverse());
+				_update_transform_dependent();
 			}
 			wakeup();
 
@@ -651,7 +652,7 @@ void GodotBody3D::integrate_velocities(real_t p_step) {
 	_set_transform(transform);
 	_set_inv_transform(get_transform().inverse());
 
-	_update_transform_dependant();
+	_update_transform_dependent();
 }
 
 void GodotBody3D::wakeup_neighbours() {
diff --git a/servers/physics_3d/godot_body_3d.h b/servers/physics_3d/godot_body_3d.h
index 1151a22c96..5acdab9d13 100644
--- a/servers/physics_3d/godot_body_3d.h
+++ b/servers/physics_3d/godot_body_3d.h
@@ -135,9 +135,9 @@ class GodotBody3D : public GodotCollisionObject3D {
 
 	uint64_t island_step = 0;
 
-	_FORCE_INLINE_ void _compute_area_gravity_and_damping(const GodotArea3D *p_area);
+	void _compute_area_gravity_and_damping(const GodotArea3D *p_area);
 
-	_FORCE_INLINE_ void _update_transform_dependant();
+	void _update_transform_dependent();
 
 	friend class GodotPhysicsDirectBodyState3D; // i give up, too many functions to expose