Fix physics body rotating incorrectly around it's center of mass

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