diff options
Diffstat (limited to 'servers/physics_2d/godot_body_2d.cpp')
| -rw-r--r-- | servers/physics_2d/godot_body_2d.cpp | 21 |
1 files changed, 9 insertions, 12 deletions
diff --git a/servers/physics_2d/godot_body_2d.cpp b/servers/physics_2d/godot_body_2d.cpp index 68f114a34a..56f191c203 100644 --- a/servers/physics_2d/godot_body_2d.cpp +++ b/servers/physics_2d/godot_body_2d.cpp @@ -55,7 +55,7 @@ void GodotBody2D::update_mass_properties() { if (calculate_center_of_mass) { // We have to recompute the center of mass. - center_of_mass = Vector2(); + center_of_mass_local = Vector2(); if (total_area != 0.0) { for (int i = 0; i < get_shape_count(); i++) { @@ -68,10 +68,10 @@ void GodotBody2D::update_mass_properties() { real_t mass = area * this->mass / total_area; // NOTE: we assume that the shape origin is also its center of mass. - center_of_mass += mass * get_shape_transform(i).get_origin(); + center_of_mass_local += mass * get_shape_transform(i).get_origin(); } - center_of_mass /= mass; + center_of_mass_local /= mass; } } @@ -94,7 +94,7 @@ void GodotBody2D::update_mass_properties() { Transform2D mtx = get_shape_transform(i); Vector2 scale = mtx.get_scale(); - Vector2 shape_origin = mtx.get_origin() - center_of_mass; + Vector2 shape_origin = mtx.get_origin() - center_of_mass_local; inertia += shape->get_moment_of_inertia(mass, scale) + mass * shape_origin.length_squared(); } } @@ -276,6 +276,7 @@ PhysicsServer2D::BodyMode GodotBody2D::get_mode() const { void GodotBody2D::_shapes_changed() { _mass_properties_changed(); + wakeup(); wakeup_neighbours(); } @@ -557,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); |