Fix 2D center of mass not updated from transform

Same logic as what was done in 3D, applied to 2D center of mass.

Also did some minor cleanup in 3D and fixed center of mass transform
during the first frame after teleporting a dynamic body.

2 files changed, 11 insertions, 10 deletions

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