diff --git a/servers/physics_2d/godot_body_pair_2d.cpp b/servers/physics_2d/godot_body_pair_2d.cpp index 6c2d28dc923..1ff50cf9ab4 100644 --- a/servers/physics_2d/godot_body_pair_2d.cpp +++ b/servers/physics_2d/godot_body_pair_2d.cpp @@ -403,7 +403,6 @@ bool GodotBodyPair2D::pre_solve(real_t p_step) { bool do_process = false; - const Vector2 &offset_A = A->get_transform().get_origin(); const Transform2D &transform_A = A->get_transform(); const Transform2D &transform_B = B->get_transform(); @@ -428,6 +427,7 @@ bool GodotBodyPair2D::pre_solve(real_t p_step) { } #ifdef DEBUG_ENABLED + const Vector2 &offset_A = A->get_transform().get_origin(); if (space->is_debugging_contacts()) { space->add_debug_contact(global_A + offset_A); space->add_debug_contact(global_B + offset_A); @@ -458,16 +458,8 @@ bool GodotBodyPair2D::pre_solve(real_t p_step) { c.acc_impulse -= P; - if (A->can_report_contacts() || B->can_report_contacts()) { - Vector2 crB = Vector2(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x) + B->get_linear_velocity(); - Vector2 crA = Vector2(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x) + A->get_linear_velocity(); - if (A->can_report_contacts()) { - A->add_contact(global_A + offset_A, -c.normal, depth, shape_A, crA, global_B + offset_A, shape_B, B->get_instance_id(), B->get_self(), crB, c.acc_impulse); - } - if (B->can_report_contacts()) { - B->add_contact(global_B + offset_A, c.normal, depth, shape_B, crB, global_A + offset_A, shape_A, A->get_instance_id(), A->get_self(), crA, c.acc_impulse); - } - } + c.initial_rB = Vector2(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x) + B->get_linear_velocity(); + c.initial_rA = Vector2(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x) + A->get_linear_velocity(); if (report_contacts_only) { collided = false; @@ -591,6 +583,37 @@ void GodotBodyPair2D::solve(real_t p_step) { } } +void GodotBodyPair2D::post_solve(real_t p_step) { + for (int i = 0; i < contact_count; ++i) { + Contact &c = contacts[i]; + + if (!c.active) { + continue; + } + + if (A->can_report_contacts() || B->can_report_contacts()) { + const Vector2 &offset_A = A->get_transform().get_origin(); + + const Transform2D &transform_A = A->get_transform(); + const Transform2D &transform_B = B->get_transform(); + + Vector2 global_A = transform_A.basis_xform(c.local_A); + Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B; + + Vector2 axis = global_A - global_B; + real_t depth = axis.dot(c.normal); + + if (A->can_report_contacts()) { + A->add_contact(global_A + offset_A, -c.normal, depth, shape_A, c.initial_rA, global_B + offset_A, shape_B, + B->get_instance_id(), B->get_self(), c.initial_rB, c.acc_impulse); + } + if (B->can_report_contacts()) { + B->add_contact(global_B + offset_A, c.normal, depth, shape_B, c.initial_rB, global_A + offset_A, shape_A, + A->get_instance_id(), A->get_self(), c.initial_rA, c.acc_impulse); } + } + } +} + GodotBodyPair2D::GodotBodyPair2D(GodotBody2D *p_A, int p_shape_A, GodotBody2D *p_B, int p_shape_B) : GodotConstraint2D(_arr, 2) { A = p_A; diff --git a/servers/physics_2d/godot_body_pair_2d.h b/servers/physics_2d/godot_body_pair_2d.h index 4e9bfa6022a..4286bc8ce95 100644 --- a/servers/physics_2d/godot_body_pair_2d.h +++ b/servers/physics_2d/godot_body_pair_2d.h @@ -70,6 +70,7 @@ class GodotBodyPair2D : public GodotConstraint2D { real_t depth = 0.0; bool active = false; bool used = false; + Vector2 initial_rA, initial_rB; Vector2 rA, rB; real_t bounce = 0.0; }; @@ -93,6 +94,7 @@ class GodotBodyPair2D : public GodotConstraint2D { virtual bool setup(real_t p_step) override; virtual bool pre_solve(real_t p_step) override; virtual void solve(real_t p_step) override; + virtual void post_solve(real_t p_step) override; GodotBodyPair2D(GodotBody2D *p_A, int p_shape_A, GodotBody2D *p_B, int p_shape_B); ~GodotBodyPair2D(); diff --git a/servers/physics_2d/godot_constraint_2d.h b/servers/physics_2d/godot_constraint_2d.h index f4136f66436..8dd7a7d1368 100644 --- a/servers/physics_2d/godot_constraint_2d.h +++ b/servers/physics_2d/godot_constraint_2d.h @@ -63,6 +63,7 @@ class GodotConstraint2D { virtual bool setup(real_t p_step) = 0; virtual bool pre_solve(real_t p_step) = 0; virtual void solve(real_t p_step) = 0; + virtual void post_solve(real_t p_step) {} virtual ~GodotConstraint2D() {} }; diff --git a/servers/physics_2d/godot_step_2d.cpp b/servers/physics_2d/godot_step_2d.cpp index bbaec8be2bf..6a8955bb233 100644 --- a/servers/physics_2d/godot_step_2d.cpp +++ b/servers/physics_2d/godot_step_2d.cpp @@ -101,6 +101,13 @@ void GodotStep2D::_solve_island(uint32_t p_island_index, void *p_userdata) const } } +void GodotStep2D::_post_solve_island(LocalVector &p_constraint_island) const { + uint32_t constraint_count = p_constraint_island.size(); + for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) { + p_constraint_island[constraint_index]->post_solve(delta); + } +} + void GodotStep2D::_check_suspend(LocalVector &p_body_island) const { bool can_sleep = true; @@ -269,6 +276,11 @@ void GodotStep2D::step(GodotSpace2D *p_space, real_t p_delta) { profile_begtime = profile_endtime; } + // Warning: This doesn't run on threads, because it involves thread-unsafe processing. + for (uint32_t island_index = 0; island_index < island_count; ++island_index) { + _post_solve_island(constraint_islands[island_index]); + } + /* INTEGRATE VELOCITIES */ b = body_list->first(); diff --git a/servers/physics_2d/godot_step_2d.h b/servers/physics_2d/godot_step_2d.h index c08c6379de3..6c5c8a828dc 100644 --- a/servers/physics_2d/godot_step_2d.h +++ b/servers/physics_2d/godot_step_2d.h @@ -49,6 +49,7 @@ class GodotStep2D { void _setup_constraint(uint32_t p_constraint_index, void *p_userdata = nullptr); void _pre_solve_island(LocalVector &p_constraint_island) const; void _solve_island(uint32_t p_island_index, void *p_userdata = nullptr) const; + void _post_solve_island(LocalVector &p_constraint_island) const; void _check_suspend(LocalVector &p_body_island) const; public: