From c1577e5a3f97b15fa464b0b94b9172b20835c33a Mon Sep 17 00:00:00 2001 From: PouleyKetchoupp Date: Tue, 19 Oct 2021 16:16:00 -0700 Subject: [PATCH] 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. --- servers/physics_2d/godot_body_2d.cpp | 12 +++++++++++- servers/physics_2d/godot_body_2d.h | 5 ++++- servers/physics_3d/godot_body_3d.cpp | 17 +++++++++-------- servers/physics_3d/godot_body_3d.h | 4 ++-- 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 b0885a1f7bd..68f114a34a7 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 c4f3578f1b7..5fce362fa7d 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 6df6a0c45ba..0564c4d4522 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 1151a22c961..5acdab9d13e 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