From 2275e264766d7866b52c573c8a10333f9916794f Mon Sep 17 00:00:00 2001 From: Uxeron Date: Fri, 22 Oct 2021 17:07:45 +0300 Subject: Fix physics body rotating incorrectly around it's center of mass --- servers/physics_2d/godot_body_2d.cpp | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/servers/physics_2d/godot_body_2d.cpp b/servers/physics_2d/godot_body_2d.cpp index a18c748e1d..171e5969f6 100644 --- a/servers/physics_2d/godot_body_2d.cpp +++ b/servers/physics_2d/godot_body_2d.cpp @@ -558,17 +558,13 @@ void GodotBody2D::integrate_velocities(real_t p_step) { real_t total_angular_velocity = angular_velocity + biased_angular_velocity; Vector2 total_linear_velocity = linear_velocity + biased_linear_velocity; - real_t angle = get_transform().get_rotation() + total_angular_velocity * p_step; + real_t angle_delta = total_angular_velocity * p_step; + real_t angle = get_transform().get_rotation() + angle_delta; 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) { + if (center_of_mass.length_squared() > CMP_EPSILON2) { // 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); + pos += center_of_mass - center_of_mass.rotated(angle_delta); } _set_transform(Transform2D(angle, pos), continuous_cd_mode == PhysicsServer2D::CCD_MODE_DISABLED); -- cgit v1.2.3